JP2011164069A - Position correction system - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To obtain a position correction system capable of preventing position error magnification of a moving object, by correcting a moving object's position, as well as, a moving object's angle detected by an angle detection means. <P>SOLUTION: The position correction system includes a position detection means for detecting the present position of a vehicle 1; a gyroscope 24 for detecting an angle of the vehicle 1 to a given reference; a feature position data 35 where positions of features 2, in a travel range of the vehicle 1 are recorded in advance; a camera 21 for taking an image of the surrounding of the vehicle 1; a laser range finder 22 for measuring the distance between the vehicle 1 and the surrounding environment; and a position correcting unit 12 which finds the relative relation between the vehicle 1 and a specific feature 2, from measurement result of the specific feature 2 within the camera image and recorded in the feature position data 35 by using the laser range finder 22, corrects the present position of the vehicle 1, detected by the position detection means, based on the relative relationship, and corrects the angle of the vehicle 1 detected by the gyroscope 24, based on multiple relative relations. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a position correction system that corrects the position of a vehicle detected by, for example, GPS.

  Currently, for outdoor positioning, a global positioning system (GPS) has become the mainstream. The GPS receives a GPS signal from a GPS satellite and detects the position of a moving body (for example, a vehicle) on the earth based on the received GPS signal.

  However, in urban areas where there are many high-rise buildings, GPS satellites may not be seen from the measurement location, for example, in valleys of buildings, and the measurement environment may deteriorate. If the measurement environment deteriorates, the vehicle position may not be detected at all, or the measurement error may increase.

  Therefore, in an environment where the GPS signal reception condition is poor, it is necessary to detect the position of the vehicle by another method. Generally, when a device such as a gyroscope that detects a vehicle angle (traveling direction) or angular velocity and an acceleration sensor that detects vehicle acceleration is separately mounted on the vehicle and the position of the vehicle cannot be detected by GPS, the gyroscope The position of the vehicle is calculated based on outputs from the scope and the acceleration sensor.

  At this time, the relative positional relationship between the vehicle and the surrounding features is calculated based on the output from the camera and the laser mounted on the vehicle, and the gyroscope and the acceleration sensor are calculated using the calculated relative positional relationship. A current position detection device that corrects the position of a vehicle calculated based on an output has been proposed (see, for example, Patent Document 1).

JP 2007-232690 A

However, the prior art has the following problems.
In the conventional current position detection device, the position of the vehicle can be corrected, but the angle (traveling direction) or angular velocity of the vehicle cannot be corrected. This is because the means for detecting the angle (or angular velocity) of the vehicle depends only on the gyroscope.

  In other words, the conventional current position detection device can correct the position of the vehicle at the “correction moment”, but if the vehicle angle detected by the gyroscope includes an error, the correction of the vehicle position is performed. Later, there is a problem that an error occurs in the position of the vehicle immediately. This error increases as the travel distance increases.

  Furthermore, devices such as gyroscopes have the problem of error accumulation, which increases the error during use. Therefore, if the error of the gyroscope is not eliminated in some way, there is a problem that the measurement error increases.

  The present invention has been made in order to solve the above-described problems, and corrects the position of the moving body and corrects the angle of the moving body detected by the angle detection means, thereby correcting the position error of the moving body. An object of the present invention is to obtain a position correction system that can prevent the enlargement of the image.

  The position correction system according to the present invention includes a position detection unit that detects a current position of a moving body, an angle detection unit that detects an angle of the moving body with respect to a predetermined reference, and a position of a feature in a moving range of the moving body in advance. Recorded feature position recording means, photographing means for photographing the surroundings of the moving body, distance measuring means for sending a laser around the moving body and measuring the distance between the moving body and the surrounding environment, and photographing means In the image photographed in step 1, the relative relationship between the moving object and the specific feature is obtained from the measurement result of the distance measurement unit with respect to the specific feature recorded in the feature position recording unit, and the position is detected based on the relative relationship. And a position correcting means for correcting the current position of the moving body detected by the means and correcting the angle of the moving body detected by the angle detecting means based on a plurality of relative relationships.

According to the position correction system of the present invention, the position correction unit is configured to detect the moving object from the measurement result by the distance measurement unit for the specific feature recorded in the feature position recording unit in the image captured by the imaging unit. A relative relationship with a specific feature is obtained, and the angle of the moving body detected by the angle detection means is corrected based on the plurality of relative relationships.
Therefore, it is possible to obtain a position correction system capable of correcting the position of the moving body and correcting the angle of the moving body detected by the angle detection unit to prevent the position error of the moving body from being enlarged.

It is a block diagram which shows the vehicle carrying the position correction system which concerns on Embodiment 1 of this invention. It is a block block diagram which shows the whole system containing the position correction system which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the positional relationship at the time of performing vehicle correction twice in the vehicle carrying the position correction system which concerns on Embodiment 1 of this invention. It is a flowchart which shows operation | movement of the position correction part of the position correction system which concerns on Embodiment 1 of this invention. In the position correction system which concerns on Embodiment 1 of this invention, explanatory drawing which shows the calculation formula showing in which coordinate in the camera image the point group represented by the coordinate system which made the optical axis center of the camera the origin is reflected It is. Description of a calculation formula for calculating an error value when ε is an angle error of a gyroscope at the time of conversion from a vehicle coordinate system to a plane rectangular coordinate system in the position correction system according to Embodiment 1 of the present invention. FIG. It is explanatory drawing for demonstrating the effect of the position correction system which concerns on Embodiment 1 of this invention. It is explanatory drawing for demonstrating the effect of the position correction system which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the effect of the position correction system which concerns on Embodiment 1 of this invention compared with a prior art.

Hereinafter, preferred embodiments of the position correction system of the present invention will be described with reference to the drawings. In the drawings, the same or corresponding parts will be described with the same reference numerals.
In the following embodiments, a case will be described in which this position correction system is mounted on a vehicle to be measured.

Embodiment 1 FIG.
FIG. 1 is a configuration diagram showing a vehicle 1 (moving body) equipped with a position correction system according to Embodiment 1 of the present invention. FIG. 2 is a block configuration diagram showing the entire system including the position correction system according to Embodiment 1 of the present invention.

  1 and 2, this system includes a control device 10, a camera 21 (imaging means), a laser range finder 22 (distance measurement means), a GPS receiver 23 (position detection means), a gyroscope 24 (angle detection means), Camera photographing data 31, camera photographing time data 32, distance measurement data 33, distance measurement time data 34, and feature position data 35 (feature position recording means) are provided.

  The camera 21 periodically photographs the front of the vehicle 1, outputs the photographed image to the control device 10 and records it in the camera photographing data 31, and records the photographing time in the camera photographing time data 32. The laser range finder 22 sends a laser in front of the vehicle 1, measures the distance between the vehicle 1 and the surrounding environment, outputs the measured distance to the control device 10, records it in the distance measurement data 33, and measures the distance. The time is recorded in the distance measurement time data 34.

At this time, the laser range finder 22 periodically changes the angle at which the laser is transmitted to cover the entire front of the vehicle 1.
The frequency of distance measurement with the surrounding environment by the laser range finder 22 is assumed to be higher than the frequency of photographing the vehicle 1 in front of the camera 21. In general, the laser range finder 22 scans around the vehicle 1 50 times or more per second, and the camera 21 captures the front of the vehicle 1 about 5 times per second.

  The GPS receiver 23 receives a GPS signal from a GPS satellite (not shown), and outputs the received GPS signal to the control device 10. The gyroscope 24 detects an angle (traveling direction) or angular velocity with respect to a predetermined reference of the vehicle 1 and outputs a detection result to the control device 10. Here, it is assumed that the gyroscope 24 detects the angle (traveling direction) of the vehicle 1.

  The control device 10 includes a position detection unit 11 (position detection unit) and a position correction unit 12 (position correction unit). In the feature position data 35, the position of the feature 2 in the environment in which the vehicle 1 travels is measured in advance, and the measured position information is recorded. However, since it is difficult to measure the positions of all the features, the positions are measured only for some of the features.

The position detector 11 detects the position of the vehicle 1 on the earth based on the GPS signal from the GPS receiver 23.
When the GPS signal from the GPS satellite cannot be received, the position correction unit 12 is based on the shooting result by the camera 21, the measurement result by the laser range finder 22, and the position information recorded in the feature position data 35. In addition to correcting the position of the vehicle 1 detected in step 1, the angle of the vehicle 1 detected by the gyroscope 24 is corrected.

  In addition, regarding the position detection of the vehicle 1 by the position detection unit 11, information obtained from other sensors or the like, for example, vehicle information such as the vehicle speed, the number of wheel rotations, and the handle angle may be used together. The angle detection of the vehicle 1 is not limited to the gyroscope 24. For example, a plurality of GPS receivers 23 may be mounted to detect the angle.

  Here, when attention is paid to a value that can be used as a true value in the position detection of the vehicle 1, only the position of the feature 2 recorded in the feature position data 35 can be obtained as a correct value. Therefore, if the angle of the vehicle 1 can be obtained from the position of the feature 2, the angle of the vehicle 1 detected by the gyroscope 24 can be corrected by the angle obtained from the position of the feature 2. At this time, it is clear which is more accurate between the angle obtained from the true position of the feature 2 and the angle detected by the gyroscope 24 in which errors accumulate.

  On the other hand, in view of the current state of laser measurement by the laser range finder 22, it is rare that the feature 2 is recognized only once during the measurement. This is because the vehicle speed at the time of laser measurement is relatively low (about 30 km / h), and the frequency of photographing by the camera 21 is not small (about 5 times per second). That is, a single feature 2 can be shot multiple times.

Furthermore, as described above, since the frequency of distance measurement with the surrounding environment by the laser range finder 22 is higher than the frequency of photographing the front of the vehicle 1 by the camera 21, the laser measurement result corresponding to each of a plurality of camera images. Is likely to be obtained.
That is, it is considered highly likely that the relative relationship between the vehicle 1 and the feature 2 is obtained a plurality of times for one feature 2.

  Therefore, in the first embodiment, in order to correct the angle of the vehicle 1 detected by the gyroscope 24, the angle of the vehicle 1 is calculated based on the relative relationship between the vehicle 1 and the feature 2 a plurality of times. How to do. Hereinafter, obtaining the relative relationship between the vehicle 1 and the feature 2 is referred to as “vehicle correction”.

  When the vehicle correction is performed a plurality of times for the same feature 2, the time interval for the correction (the time interval when the used laser measurement result is obtained) is about several hundred milliseconds at most. At this time, unless the vehicle 1 is about to change direction at an intersection or the like, it is unlikely that the traveling direction of the vehicle 1 will change suddenly. That is, it can be approximated that the vehicle 1 during vehicle correction is traveling straight (the traveling direction does not change).

FIG. 3 is an explanatory diagram showing the positional relationship when the vehicle correction is performed twice in the vehicle 1 equipped with the position correction system according to Embodiment 1 of the present invention.
In FIG. 3, the direction (traveling direction) of the vehicle 1 is a direction in which the position of the vehicle 1 by the first vehicle correction is connected by a straight line from the position of the vehicle 1 by the second vehicle correction. Further, the angle error can be calculated based on the distance by the second vehicle correction.

  Actually, since three angles are required in the three-dimensional space, it cannot be calculated only by FIG. 3, but it is calculated by setting up three sets of the same processing (recognizing the feature 2 four times). be able to.

Next, a specific operation of the position correction unit 12 will be described with reference to the flowchart shown in FIG.
FIG. 4 shows a flow for correcting the position and angle of the vehicle 1 based on the photographing result by the camera 21 recorded in the camera photographing data 31 and the measurement result by the laser range finder 22 recorded in the distance measurement data 33. It is a thing. Here, it is assumed that the reception sensitivity of the GPS receiver 23 is poor and the positional accuracy of the vehicle 1 may be deviated on the order of meters. It is also assumed that the gyroscope 24 has an error (less than 1 degree to several degrees).

First, the position correction unit 12 checks the camera image recorded in the camera shooting data 31 in time series, and detects the feature 2 by image processing (step S1).
Subsequently, the position correction unit 12 determines whether or not the feature 2 is present in the camera image (in front of the vehicle 1) (step S2).

If it is determined in step S2 that the feature 2 is present in the camera image (that is, Yes), the position correction unit 12 records the true position value of the feature 2 in the feature position data 35. It is determined whether or not there is (step S3).
This is performed by narrowing down the range using the position of the vehicle 1 (including a metric order error) temporarily detected by the position detection unit 11 as the position at the time of photographing by the camera 21. Specifically, since there may be a metric error, it is determined whether or not there is data that is considered to correspond to the feature position data 35 for a narrow range of about 10 meters square from the position taken by the camera 21. judge.

In step S3, when it is determined that the true position value of the feature 2 is recorded in the feature position data 35 (that is, Yes), the position correction unit 12 obtains the measurement result by the laser range finder 22 ( It is determined whether or not the measurement result obtained by the laser is converted to the three-dimensional coordinates (hereinafter referred to as “point group”) and hitting the feature 2 (step S4).
Specifically, the position of the point cloud is mapped on the camera image to calculate the position in the image, and one that falls within the range of the feature 2 is selected.

  FIG. 5 shows in the position correction system according to the first embodiment of the present invention, at which coordinates (coordinates in the image) the point group represented by the coordinate system with the optical axis center of the camera 21 as the origin is in the camera image. It is explanatory drawing which shows the calculation formula showing whether it is reflected. In the coordinate calculation of the point group, the measurement result (distance) by the laser range finder 22 is once calculated as the coordinates of the laser coordinate system, which is the coordinate system with the laser transmission position as the origin, and then the vehicle coordinates of the laser range finder 22 The installation position and the installation angle in the system are converted into the vehicle coordinate system as parameters. Based on the coordinate value converted into the vehicle coordinate system and the installation position and installation angle of the camera 21 in the vehicle coordinate system, the position of the point cloud in the camera coordinate system can be calculated.

  Here, the vehicle coordinate system is a coordinate system in which the forward direction of the vehicle 1 is the x axis, the upward direction of the vehicle 1 is the y axis, the right direction of the vehicle 1 is the z axis, and the coordinate value is equal to the distance (unit is meter). Define. The camera coordinate system is defined as a coordinate system in which the x, y, and z axes are oriented in the same direction as the vehicle coordinate system, and the lens center of the camera 21 is the origin. The laser coordinate system is defined as a coordinate system in which the x, y, and z axes are oriented in the same direction as the vehicle coordinate system and the laser transmission position is the origin.

  In step S4, it is determined that there is a point cloud that hits the feature 2 (that is, Yes), one point group that falls within the range of the feature 2 is selected, and the feature position coordinates (that is, the vehicle coordinate system) When the positional relationship between the vehicle 1 and the feature 2 is determined, the position correction unit 12 converts the feature position coordinates into an absolute distance system (planar rectangular coordinate system) (step S5).

  On the other hand, if it is determined in step S2 that there is no feature 2 in the camera image (that is, No), the true position value of the feature 2 is not recorded in the feature position data 35 in step S3 ( That is, when it is determined No) and when it is determined in step S4 that there is no point group that hits the feature 2 (that is, No), the position correction unit 12 immediately ends the correction process.

  The plane rectangular coordinate system is orthogonal from the origin in Japan (19 locations in Japan, depending on the position of the measurement target), with the latitude direction as the x-axis, the longitude direction as the y-axis, and the height direction as the z-axis. Coordinate system. The plane Cartesian coordinate system is shown in the Geospatial Information Authority of Japan, Plane Cartesian Coordinate System (Ministry of Land, Infrastructure, Transport and Tourism Notification No. 9 of 2004) (http://www.gsi.go.jp/LAW/heimencho.html). Therefore, detailed description is omitted. For example, in Tokyo (excluding islands), the origin is 139 degrees 50 minutes 0 seconds east longitude 36 degrees 0 minutes 0 seconds north latitude (Noda City, Chiba Prefecture).

Here, when converting from the vehicle coordinate system to the planar rectangular coordinate system, the orientation of the vehicle in the planar rectangular coordinate system is required.
FIG. 6 shows the conversion from the vehicle coordinate system to the plane rectangular coordinate system in the position correction system according to the first embodiment of the present invention, and the error value when the angle error of the gyroscope 24 at that time is ε. It is explanatory drawing which shows a calculation formula. However, it is assumed that the vehicle 1 is traveling vertically with respect to the ground (the vehicle coordinate system y-axis and the height direction of the plane rectangular coordinates coincide).

  As can be seen from FIG. 6, the measurement errors Δx and Δy depend on the coordinate value of the vehicle coordinate system x-axis (vehicle traveling direction), the coordinate value of the z-axis (rightward in the vehicle traveling direction), and the angle error. At this time, assuming that the angle error ε = 2 ° of the gyroscope 24 is sin (ε / 2) = about 0.01, if the distance in the x and z axis directions is about 10 m, an error of about 10-20 cm is obtained. This is small compared to the position error temporarily detected by the position detection unit 11 described above.

  Subsequently, the position correction unit 12 determines the vehicle based on the vehicle relative position of the feature 2 in the plane rectangular coordinates obtained in step S5 and the true position value of the feature 2 recorded in the feature position data 35. 1 is obtained, and the position of the vehicle 1 is corrected (step S6). When the feature position data 35 is recorded in another coordinate system, it is necessary to convert it into a plane rectangular coordinate system.

Next, the position correction unit 12 determines whether or not the position correction by the feature 2 is the second time (step S7).
In step S7, when it is determined that the position correction by the feature 2 is not the second time (the first time) (that is, No), the position correction unit 12 records the position of the vehicle 1 after the correction. (Step S8), and the process of FIG.

  On the other hand, when it is determined in step S7 that the position correction by the feature 2 is the second time (that is, Yes), the position correction unit 12 performs the first time recorded in step S8 of the previous cycle. Based on the position of the vehicle 1 at the time of position correction and the position of the vehicle 1 corrected in step S6 of the current cycle, the angle of the vehicle 1 is calculated (step S9).

  When one feature 2 can be detected a plurality of times (for example, twice), the position of the vehicle 1 at the time of each detection is about the shooting interval of the camera 21 in terms of time, and is not so far away. Further, since it is unlikely that the angle of the vehicle 1 will suddenly change at an interval of less than 1 second unless it is turning at a corner, the vehicle 1 moves linearly between the positions of the two vehicles 1 (ie, It can be assumed that the directions in the plane rectangular coordinate system are equal to each other.

Furthermore, when the vehicle 1 is moving linearly, the angle error (gyro error) of the gyroscope 24 does not fluctuate rapidly in a short time. Therefore, the measurement error due to the angle error shown in FIG. 6 is reduced as shown in FIG. 8 by the calculation formulas (X ₂ −X ₁ and Y ₂ −Y ₁ ) shown in FIG. I understand that. Here, there is little change in the vehicle position _(n r2 _{-n r1} and _e r2 _{-e r1} in FIG. 8 is close to 0), the measurement error due to the angle error is a value in the vicinity of substantially zero. Thereby, it can be seen that the angle value obtained by FIG. 7 increases the accuracy.

  Subsequently, the position correction unit 12 corrects the angle of the vehicle 1 detected by the gyroscope 24 based on the angle of the vehicle 1 calculated in step S9 (step S10), and ends the process of FIG. Thereby, the angle error (corresponding to ε shown in FIG. 5) of the gyroscope 24 is calculated, and it can be seen that the angle of the vehicle 1 is corrected.

  In this way, the angle value of the vehicle 1 can be corrected using the ground position true value and the angle value including an error. As shown in FIG. 9, the angle error appears as an increase in the position error as the position of the vehicle 1 advances. As a result, the positioning accuracy of the vehicle 1 is improved.

As described above, according to the first embodiment, the position correction unit moves from the measurement result by the distance measurement unit for the specific feature recorded in the feature position recording unit in the image captured by the imaging unit. The relative relationship between the body and the specific feature is obtained, and the angle of the moving body detected by the angle detection means is corrected based on the relative relationship of a plurality of times.
Therefore, it is possible to obtain a position correction system capable of correcting the position of the moving body and correcting the angle of the moving body detected by the angle detection unit to prevent the position error of the moving body from being enlarged.

In the first embodiment, it has been described that the relative distance between the vehicle 1 and the feature 2 is calculated using the measurement result of the laser range finder 22 and the camera image captured by the camera 21. However, the present invention is not limited to this, and the positional relationship between the vehicle 1 and the feature 2 may be obtained by stereo viewing using two images captured simultaneously using two cameras.
Also in this case, the same effect as in the first embodiment can be obtained.

In the first embodiment, the feature 2 is recognized only by the camera image. However, the present invention is not limited to this, and a point cloud may be used at the recognition stage. That is, it is possible to make a three-dimensional determination by calculating a shape using a point cloud and obtaining a color using a camera image. At this time, the positional relationship with the feature 2 can be obtained from the shape of the obtained feature 2.
Also in this case, the same effect as in the first embodiment can be obtained.

  DESCRIPTION OF SYMBOLS 1 Vehicle, 2 features, 11 Position detection part (position detection means), 12 Position correction part (position correction means), 21 Camera (photographing means), 22 Laser range finder (distance measurement means), 23 GPS receiver (position Detection means), 24 gyroscope (angle detection means), 35 feature position data (feature position recording means).

Claims (1)

Position detecting means for detecting the current position of the moving body;
Angle detecting means for detecting an angle of the moving body with respect to a predetermined reference;
Feature position recording means in which the position of the feature in the moving range of the moving body is recorded in advance;
Photographing means for photographing the periphery of the moving body;
A distance measuring means for sending a laser around the moving body and measuring a distance between the moving body and the surrounding environment;
In the image photographed by the photographing means, obtain a relative relationship between the moving object and the specific feature from the measurement result by the distance measuring means for the specific feature recorded in the feature position recording means, Based on the relative relationship, the current position of the moving body detected by the position detecting unit is corrected, and the angle of the moving body detected by the angle detecting unit is corrected based on the relative relationship a plurality of times. Position correction means;
A position correction system comprising:

Images