JP5162849B2 - Fixed point position recorder - Google Patents

Description

The present invention relates to a high-precision positioning technique using GPS, and more particularly, to an animal position recording apparatus that estimates and records the position of an animal around a host vehicle in a traveling environment with high accuracy.
INDUSTRIAL APPLICABILITY The present invention is useful as a technique for easily and accurately constructing a database of absolute position information of inanimate animals around a road, which is provided in a car navigation system, an auto cruise system, or the like to confirm the current position of the host vehicle Is.

  In recent years, RTK-GPS (Real-Time Kinematic Global Positioning System) has made it possible to obtain positioning accuracy with an error of about 2 to 3 cm by positioning technology using GPS satellites. It is expensive, and is not necessarily suitable for carrying out positioning in real time for a moving body such as a running car.

  Further, as a conventional positioning technique for implementing a relatively high-accuracy positioning related to the own vehicle in real time, which can be configured without using these RTK-GPS and the like, for example, in Patent Documents 1 and 2 below, Those described are known. In these positioning technologies, feature data and position data of inanimate objects (eg road signs, reflectors, stop lines, etc.) in detail around the road that are not used in a normal navigation system are stored in advance by a preliminary survey. Devices ("detailed map DB" and "environmental environment storage device"), and further compare the inanimate feature data stored there with the inanimate features detected in real time during travel It is characterized in that it includes a collating means that performs this.

  In particular, Patent Document 1 below extracts a lane boundary line and a landmark (such as a road surface display or a road sign) from an image taken by an in-vehicle camera in order to estimate a high-accuracy position of an automobile, and a detailed map of the road. It is characterized in that these two pieces of information are used in combination for checking the consistency with the information at the lane level.

Moreover, the following patent document 2 has the following description.
(Description 1) “By correcting the current position of the vehicle determined by the navigation device based on the fixed object data detected by the radar device and the fixed object data stored in the road surrounding environment storage device, The error of the current position of the host vehicle can be set to an error of about several tens of meters to about 10 cm to several tens of centimeters, and the accuracy can be improved. "
(Description 2) “By driving once on the highway and storing the position of a fixed object such as a reflector or an illumination lamp in the road surrounding environment storage device, it is possible to run the vehicle by the travel lane keeping device of the present invention from the next time. The lane can be maintained automatically. "
JP 2005-265494 A JP-A-10-300493

  In the positioning method of Patent Document 1 described above, it is necessary to extract lane boundary lines and landmarks (road surface display, road signs, etc.) from an image taken by an in-vehicle camera, but such image processing is not always easy. In addition, depending on the weather, time zone, and the like, it is often the case that these image processes become difficult. In addition, in order to perform the consistency check as described above, it is necessary to register in advance a predetermined landmark (road surface display, road sign, etc.) as detailed map information of the road in the “detailed map DB”. There are many roads that do not have any of these landmarks. Therefore, the positioning technique disclosed in Patent Document 1 has many problems in terms of its application range and reliability. In addition, it is not always a realistic method to register high-precision absolute positions of certain landmarks such as road surface displays and road signs in the detailed map DB one by one.

In addition, in order to realize the positioning accuracy shown in (Description 1) of Patent Document 2, highly accurate positioning data (absolute position) with almost no error is registered in advance in the road surrounding environment storage device. It is necessary to keep it. However, the above Patent Document 2 does not disclose any method or procedure for registering such high-accuracy positioning data in the road surrounding environment storage device in advance.
Furthermore, it is difficult to prepare such high-precision positioning data in view of the current general technical level, unless an expensive positioning means such as RTK-GPS is used, and RTK- Even when using GPS or the like, since the positioning must be carried out at the position of each fixed object, the road surrounding environment storage device described in (Description 1) above is prepared in advance. The work cost for doing so must be considered extremely large unless the application area (traveling area) is limited very narrowly.
Therefore, as long as the positioning technique shown in the above (Description 1) of Patent Document 2 is followed, the positioning device that accurately measures the current absolute position of the moving moving body is applied to the application area of a normal car navigation system. It is not easy to expand widely to general users, and such an approach is not practical.

  In addition, when using a normal GPS, it is generally said that the positioning accuracy is only about 10 m to 30 m. Therefore, according to the positioning technique shown in (Description 2) above in Patent Document 2, even the roadside Can accurately recognize the relative positional relationship from a moving object to a fixed object such as a reflector or illuminating lamp, but the absolute position of those fixed objects and the current vehicle The absolute position cannot be detected with higher accuracy than the GPS positioning accuracy (10 m to 30 m).

  The present invention has been made in order to solve the above-described problems, and its object is to provide a high level of inanimate animals around a road that is useful for real-time measurement of the current position (absolute position) of a moving object while traveling. The realization of an apparatus capable of easily collecting accurate positioning data at low cost.

In order to solve the above problems, the following means are effective.
That is, the first means of the present invention is the absolute position acquisition device that measures the first observation value that is the absolute position of the traveling vehicle, and the absolute position does not change in time in the traveling environment ahead of the traveling vehicle. A positioning device that extracts a fixed point and measures a relative observation value, which is a relative position of the fixed point with respect to the own vehicle, in real time, and has a higher accuracy than the first observation value from the first observation value. A fixed-point position recording device for mounting on a vehicle that sequentially obtains a first estimated value that is an absolute position as time elapses and obtains and records an absolute position of a fixed point, the first position being measured by the absolute position acquisition device. A fixed point absolute position calculating means for obtaining a second observed value that is an absolute position of the fixed point from one observed value and a relative observed value measured by the positioning device; Is the predicted absolute position of the vehicle Absolute position predicted value calculation for obtaining one predicted value and obtaining a second predicted value that is the absolute position of the fixed point at the current time from the second estimated value in the past that is an absolute position with higher accuracy than the second observed value of the fixed point Means, a first predicted value and a second predicted value obtained by the absolute position predicted value computing means, a second observed value obtained by the fixed point absolute position computing means, and a first measured by the absolute position acquisition device. Based on the observed value, the first estimated value and the second estimated value are corrected so that the error variance of the first estimated value and the second estimated value is reduced, and the first estimated value and the second estimated value are obtained. Absolute position correcting means, and an environmental feature memory for recording the second estimated value of the fixed point and the error variance of the second estimated value obtained by the absolute position correcting means as the reference absolute position and the reference error variance of the reference fixed point , respectively. Equipment ,
Search collating means for collating the second estimated value of the fixed point obtained by the absolute position correcting means with the reference absolute position of the fixed point stored in the environmental feature storage device;
When the second estimated value is determined to match the reference absolute position by the search collating means, if the error variance obtained by the absolute position correcting means is smaller than the reference error variance of the reference fixed point, it is newly obtained. And a data updating means for rewriting the old reference error variance with the obtained error variance as a new reference error variance .

  However, as the absolute position acquisition device described above, in addition to a GPS receiving device that receives a GPS signal, for example, a road-to-vehicle communication device using an optical beacon, DSRC, or the like, or an autonomous navigation system based on inertial navigation. A position acquisition device combined with a position / orientation detection device of a mold may be used. Such an autonomous position / orientation detection apparatus can be configured by a known prior art using, for example, a gyro, an acceleration sensor, a yaw rate sensor, a vehicle speed sensor, a turning angle sensor, or the like.

Moreover, various radars, cameras, etc. can be used as said positioning apparatus. Moreover, you may comprise said positioning apparatus combining them arbitrarily.
Further, the physical and specific data structure provided in the data group such as position data to be recorded in the environmental feature storage device may be arbitrary.

According to a second means of the present invention, in the first means described above, the correction by the absolute position correcting means is the first deviation of the first observed value corresponding to the first predicted value, the second observed value. The second deviation with respect to the observed value corresponding to the second predicted value is performed with a correction amount proportional to the error variance .
However, error variance indicating the likelihood of the absolute position, to the like may be determined in consideration of the reception status and the positioning accuracy at the time of performance and reliability and GPS signal of the GPS receiving apparatus, further, the It may be determined in consideration of the performance and reliability of the positioning device, the measurement situation (for example, weather, road surface noise, vehicle speed, etc.) and the positioning accuracy at that time. Further, the certainty (estimation accuracy and its reliability) may be expressed by using, for example, a variance or a standard deviation when an averaging process related to an absolute position is performed.

Features of the fixed point of the upper SL may be identified only by the arrangement of a plurality of fixed point (positional relationship). Therefore, in this case, it is sufficient to store only the information on the absolute positions of a number of fixed points in the environmental feature storage device as the fixed point features. For example, when a radar is used, the reflection intensity or reflectance from the detected object may be stored. Such supplementary data representing the characteristics of the fixed point can be arbitrarily added.

Further, in the present invention, a discriminating unit that discriminates an inanimate object and a moving object indicated by position data may be provided.
However, this discrimination processing may be performed under the assumption that most of the objects detected by the positioning device are, for example, non-animals.
By the above means of the present invention, the above-mentioned problem can be effectively or rationally solved.

The effects obtained by the above-described means of the present invention are as follows.
GPS signals are received for a large number of moving points whose positions can be converted to the same fixed point, and the absolute position of each moving point given by the GPS signal is converted into the position coordinates (absolute position) of the fixed point, By averaging the absolute position after conversion, random errors included in the absolute position indicated by the GPS signal can be eliminated. The fixed point may be a point indicating an inanimate animal in the driving environment around the road, or one point on the driving locus of the host vehicle may be arbitrarily selected. Moreover, the data which gives said position conversion can be acquired by the positioning means of said invention.

  That is, according to the first means of the present invention, it is possible to obtain a series of relative movements of the own vehicle with respect to the same fixed point (such as a yaw angular velocity and a velocity vector at each time and a running locus that is an integral thereof). If this motion is accurately obtained, the relative positional relationship between the fixed point and the host vehicle at each time during the series of motions can be clarified. For this reason, the absolute position of the same fixed point is changed multiple times by changing the observation time based on the absolute positions of the own vehicle at different locations given by GPS signals received multiple times during the series of movements. Can be sought.

  For this reason, the number of times of detection of the absolute position of the own vehicle during the above-described series of movements (that is, the number of times of receiving the GPS signal) is increased, and the absolute position of the same fixed point is estimated many times at the same time. If the value (absolute position of the animal) is appropriately averaged, position errors (random noise) included in the absolute position obtained from the GPS signal can be effectively eliminated based on the averaging action. . Therefore, according to the first means of the present invention, based on the absolute position of the host vehicle obtained from the GPS signal, the absolute position of the inanimate around the road can be measured much more easily and with higher accuracy than before. Can be recorded.

  Of course, it is desirable that the number of the multiple reception times for receiving the GPS signal that gives the absolute position of the host vehicle by the GPS receiving device is larger. However, within a certain number of times or more, the absolute position of the fixed point is not limited. The estimation accuracy is saturated at a certain level. However, this saturation level is caused by the positioning error of the positioning device as a limiting factor.

Further, according to the first means of the present invention, it is possible to reduce or eliminate the offset error of the absolute GPS position.
The absolute position offset error obtained from GPS signals depends on, for example, the positioning of GPS satellites, atmospheric pressure fluctuations, the ionosphere and troposphere conditions of the day, and so on, at least during a short period of minutes. Indicates a substantially constant value. For this reason, such an offset error cannot be reduced or eliminated when the absolute position of the own vehicle in a short observation period in seconds is averaged.

However, since these offset errors are also so-called random noises in the long term, it is possible to record a large number of absolute positions of the same fixed point over a long period of time in the above environmental feature storage device, or equivalent as positional information. However, if the absolute position of the same fixed point averaged over a long period of time is recorded in the environmental feature storage device, it is included in the absolute position of the fixed point obtained based on the GPS signal. It is possible to effectively reduce or eliminate the offset error.
That is, if the measurement date and time are changed, the absolute position of the same fixed point is observed over a long period of time, and the collected data is reevaluated (averaging), the inanimate position recording apparatus of the present invention Thus, it is possible to effectively reduce or eliminate the offset error included in the absolute position of the fixed point in the traveling environment.

In addition, the environmental features (that is, the identity of the set of fixed points) used in the present invention are high-level attributes having specific meanings (for example, intersections, road signs, reflectors, predetermined road surface displays, etc.). It is not limited to an object feature that can identify what it is, so when applying the present invention, the appearance frequency and detection of environmental features that are necessary to identify the positions of fixed points around the road A driving environment where the number cannot be sufficiently obtained is hardly conceivable except for off-road such as deserts and grasslands. In other words, in a driving environment where high precision positioning accuracy is required, position recognition is usually performed even if the set of fixed points (unanimals) is not known. There are many possible environmental features (animals).
Therefore, according to the first means of the present invention, it is very easy to greatly expand the area where the position data of the fixed points stored in the environmental feature storage device can be applied. In addition, since such data collection can be automatically executed, the end user can also perform the work of collecting data to be stored in the environmental feature storage device.

  In addition, according to the first means of the present invention, the motion estimation processing of the own vehicle uses the positioning device that has been frequently used in other applications in recent years, and the position obtained from these positioning devices. Since it can be executed with high accuracy based on data, the fixed point position recording apparatus of the present invention does not need to include an expensive azimuth sensor such as a gyro. Of course, it is not necessary to introduce an expensive positioning system such as the above-mentioned RTK-GPS. Therefore, according to the first means of the present invention, a desired fixed point position recording apparatus can be realized at a very low cost, and it is advantageous in terms of mountability.

  In addition, since the estimation accuracy of the absolute position of each fixed point output by the fixed point position recording apparatus of the present invention can be known, the absolute position can be used more appropriately. That is, for example, when the absolute position of the detected fixed point is stored in a database or the like, and when the data is updated or re-evaluated, the data weight, accuracy, reliability, etc. are always maintained. It becomes possible to grasp accurately. In other words, it contributes to the use of fixed point position data, subsequent reuse, and subsequent statistical operations.

  In addition, for example, when recalculating the optimal solution for the absolute position of the vehicle and the fixed point using a recursive algorithm such as the Kalman filter, the estimation accuracy (eg, variance) is updated as needed. Since it is necessary to (re-evaluate), it is also very useful for simply and sequentially calculating these update processes.

Further, according to this onset bright, averaging about absolute position of the fixed point which is stored in the environment wherein the storage device (reevaluation process) be performed in parallel in real time during the observation possible and Become. For this reason, if the above-described search matching means is used in real time during observation, the calculation time for executing the averaging process is larger than when batching such averaging processes all at once later. In addition, it is possible to effectively save the storage capacity required for the environmental feature storage device.

  In addition, the search matching means as described above is indispensable for the application when the data in the environmental feature storage device is used by the end user who uses the application such as a high-accuracy car navigation system. It becomes. Therefore, for example, as described above, when the end user himself / herself also collects data to be stored in the environmental feature storage device, the search matching unit is used in the application system used by the end user. Very useful.

Further, according to this onset bright, or can improve or maintain the reliability of data in said environment, wherein the storage device, the measurement accuracy. In an actual driving environment, vehicles are parked on the road, and the number of vehicles parked and their arrangement frequently change. Moreover, a guardrail may be newly installed or the road shape itself may change due to road construction. However, if the above-described data updating means is used, it is possible to flexibly cope with such changes in environmental characteristics by the data updating action. In addition, by increasing the number of measurement of the fixed point at the same point, it is possible to steadily improve the certainty of the position accuracy and the like.

  Further, in the present invention, when estimating the motion of the host vehicle using the captured image of the camera or the received information of the radar, it is usually assumed that most of the objects detected by these positioning devices are inanimate. In addition, these movements are calculated. For this reason, if the moving object and the non-animal in the detection data are separated by using the discriminating means before executing the movement estimation process of the own vehicle, the estimation accuracy of the own vehicle and the estimation result thereof Reliability can be effectively improved.

Therefore, by using the discriminating means, it is possible to effectively improve the accuracy and reliability of the absolute position of the inanimate animal accumulated in the desired environmental feature storage device.
Further, if such a discriminating means is used, it is also possible to simultaneously and accurately obtain the position and motion of the moving object detected during traveling.

Note that statistical operations (such as averaging processing) relating to the absolute position in the present invention can be rationally executed in real time using, for example, an extended Kalman filter, but these statistical operations use an extended Kalman filter. It is not limited to. In addition, for example, a method of sequentially updating desired inanimate data using a particle filter or the like may be used, or an optimal solution may be obtained using observation data for a certain past time.
That is, the calculation processing form such as the case of re-evaluating the accuracy of the absolute position of each inanimate may be arbitrary, and the re-evaluation processing may be executed by, for example, batch processing, or in real time at any time during measurement. You may go.

The positioning device can be configured using a laser radar, a laser range finder, a millimeter wave radar, a microwave radar, an ultrasonic sensor, a camera, or the like. The light receiving band of the camera is not limited to visible light, and may be near red or far red, and a plurality of cameras may be used like a stereo camera. Further, the positioning device of the present invention may be configured by arbitrarily combining these sensors.
As the GPS receiver to be used, a device generally used in a car navigation system or the like in recent years is sufficient. However, if a more accurate and robust device can be mounted, they may be used.

  Examples of the position data collected for grasping the characteristics and position of inanimate animals include a range profile (distance information to surrounding objects), a reflection intensity map, a stationary object map, the position and movement of a moving object, and the background. Pattern information, optical flow, edge information, and the like may be used, and Haar-like features, SIFT features, color features, and their data maps (spatial distribution information) may be used. Further, these pieces of information may be arbitrarily combined, or may be arbitrarily selected, integrated, or analyzed.

Hereinafter, the present invention will be described based on specific examples.
However, the embodiments of the present invention are not limited to the following examples.

FIG. 1 shows a logical structure of the animal position recording apparatus 100 according to the first embodiment. In the present inanimate position recording apparatus 100, the absolute position of the inanimate area around the road on which the vehicle on which the apparatus is mounted travels is used as a desired environmental feature in the storage device 160 (hereinafter referred to as the environmental feature storage device 160). )) Is the main purpose.
The animal position recording device 100 acquires a host vehicle position acquisition device 110 (GPS receiver) that acquires the absolute position of the host vehicle, and position information (including environmental features) of an object around the running road. Environmental feature acquisition device 120 (positioning device), own vehicle motion estimation unit 140 that estimates the motion (velocity vector, yaw angular velocity, etc.) of the host vehicle from the detected environment features, and the estimated host vehicle motion and host vehicle Based on the absolute position, there is an animal position estimation unit 130 that estimates and outputs the absolute position of the animal around the road.

  Furthermore, the inanimate position estimation unit 130 includes a host vehicle acquisition position evaluation unit 131 and an inanimate position evaluation unit 132. The host vehicle motion estimation unit 140 includes an environmental feature evaluation unit 141 and a host vehicle motion evaluation unit 142. Each of these evaluation units (131, 132, 141, 142) can access the primary storage device 150, respectively.

The host vehicle position acquisition device 110 can be implemented by a general GPS receiver device that has been recently used in car navigation systems, for example.
FIG. 2 illustrates a conceptual configuration diagram of the environmental feature acquisition device 120 (positioning device). The environmental feature acquisition apparatus 120 includes peripheral monitoring sensors such as a millimeter wave radar 121, a laser radar 122, an ultrasonic sensor 123, and a camera 124, and signals processed by combining signals obtained from these sensors singly or arbitrarily. It has a processing part (120a-120k). Of course, the information that can be acquired by each of these sensors is different, and naturally the nature of each environmental feature that can be extracted from the acquired signal is also different.

  For example, a range profile is distance information to an object existing in each direction, and a reflection intensity map is a projection of received radio waves and light intensities in a three-dimensional space. The Haar-like feature and SIFT feature are feature points based mainly on the luminance pattern in the image. None of the features is necessarily enough information to identify what kind of object the object is, but the location and distribution of these detected features are specific to the observed location. Therefore, these environmental features are clues for estimating the position and motion of the host vehicle.

  In addition, a selection / integration / analysis unit 128 is provided at a position as shown in the figure to select or integrate the above different types of environmental features to generate new necessary information, These environmental features may be converted into other feature amounts using component analysis or the like. Furthermore, the shape and distribution of these features may be learned as needed using, for example, a subspace method.

FIG. 3A illustrates a survey scene by the inanimal position recording apparatus 100. This photograph shows a scene in which one pedestrian crosses a pedestrian crossing of a road having a sidewalk and a guardrail on both the left and right road sides. The data in FIG. 3B is detected in the traveling scene of the host vehicle in FIG. 3-A using a device equivalent to the laser radar 122 in FIG. 2, and indicates the range profile in front of the host vehicle. In other words, this range profile is a bird's-eye view showing the position of a fixed point (laser beam reflection point) on the surface of an inanimate animal, and the positive direction of the z-axis in FIG. The positive direction of the x axis indicates the right direction of the host vehicle.
The environment feature storage device 160 in FIG. 1 can store such data (range profile), for example, and the x-axis coordinates and z-axis coordinates of such data also record the inanimate position. Specific absolute coordinates can be given based on the following operations performed by the apparatus 100.

Hereinafter, the operation of the animal position recording apparatus 100 (FIG. 1) according to the first embodiment will be described more specifically. The own vehicle acquisition position evaluation unit 131 is given by the GPS signal based on the performance of the own vehicle position acquisition device 110 (the GPS reception device), the acquisition status of the GPS signal depending on, for example, the arrangement of GPS satellites, and the like. Obtain the accuracy (eg, variance) of the absolute position of the vehicle.
The environmental feature evaluation unit 141 calculates the accuracy and reliability of the information about the types of environmental features acquired by the environmental feature acquisition device 120 and relative positions.
The coordinates used to express each of these positions may be two-dimensional coordinates in a fixed coordinate system or may be latitude / longitude, and the expression method may be arbitrary as long as these positions can be uniquely determined.

The own vehicle motion evaluation unit 142 is based on the temporal change of the positional information of the environmental feature evaluated by the environmental feature evaluation unit 141, and the motion (velocity vector or yaw angular velocity) of the own vehicle that has moved during the period in which the change was observed. Etc.). At the same time, the accuracy and reliability of the motion are calculated.
Based on the absolute position of the host vehicle, the environmental feature position data (relative position with respect to the host vehicle), the estimated movement of the host vehicle, etc. Ask. The absolute position of the inanimate to be obtained may be about 1 to 4 points.
Information such as absolute position and relative position calculated by each evaluation unit (131, 132, 141, 142), and their accuracy and reliability is stored in the primary storage device 150 only for a certain period. However, this period may be managed based on the time or based on the usage status of the primary storage device 150.

In this way, in each of the evaluation units (131, 132, 141, 142), in addition to the position (relative position or absolute position) of the host vehicle or the inanimate object to be calculated, there is a certainty regarding those positions ( (Accuracy or reliability) is also calculated. This certainty can be calculated based on the resolution and detection status of the sensor, the type of environmental feature, the number of updates, and the like.
Therefore, the primary storage device 150 in FIG. 1 temporarily stores, for example, the absolute position of the host vehicle obtained from the GPS reception signal and the probability related to the position, and the outside sensor (millimeter wave radar 121, laser). The relative position and absolute position of environmental features (non-animals) relative to the host vehicle obtained from the radar 122, the ultrasonic sensor 123, and the camera 124), and the features and likelihoods related to these positions are also temporarily stored.

  According to such a configuration, the recursive algorithm such as the Kalman filter is used, and the optimal solution such as the absolute position and the probability of the own vehicle or the desired inanimate object is sequentially processed by real-time processing in a Markov chain. Can be calculated. That is, for example, the absolute position and the certainty of the host vehicle and the environmental feature are the data stored in the primary storage device 150 (ie, each absolute position and its And the current position data output from the evaluation units 131 and 141. Therefore, in order to estimate the absolute positions and the certainty of the own vehicle and the environmental features, at least the position and the certainty of one hour ago may be held on the primary storage device 150. Also, it is sufficient that information about environmental features is retained for a period during which they can be observed.

According to the animal position recording apparatus 100 as described above, when the offset error included in the absolute position of the host vehicle obtained by GPS can be ignored, it is useful for real-time measurement of the current position (absolute position) of the moving object. It is possible to easily collect positioning data (absolute position) regarding inanimate animals around the road. Usually, the error included in the absolute position of the host vehicle obtained from the GPS signal is dominant due to random noise, and the offset error is overwhelming when it is smaller than the random error.
In addition, if the offset error included in the signal of the absolute position of the vehicle given by GPS cannot be ignored, these positioning data are collected many times, changing the measurement date and time, and the position data is averaged appropriately. Can be processed.

  FIG. 4 exemplifies a procedure for re-evaluating environmental feature data by such averaging processing. The data 160a in the figure shows the position data measured at the first time (n = 1) among the positioning data (absolute position) regarding the above-mentioned non-animals collected by changing the measurement date and time many times. Similarly, data 160b indicates position data measured for the second time (n = 2). Since each of these position data records the accuracy (accuracy and reliability) of each data at the time of positioning, if the average value of the absolute position of each non-animal is calculated based on the accuracy, It is possible to obtain the environmental feature information 160A in which the offset error is effectively reduced or eliminated from the absolute position of the animal.

  Therefore, for example, if the range profile data as described above is collected many times, changing the measurement date and time, and the position data is appropriately averaged based on the above processing method, the above-described range profile data is obtained. Since the offset error can be eliminated, the absolute position of each inanimate displayed by the absolute coordinates of the range profile data can have high reliability and sufficiently high accuracy of about 1 m, for example. Even if the number of times of data collection has not reached a sufficiently large number, if the number of times of collection and the certainty of data are constantly maintained and updated, the GPS signal is automatically given in real time. The offset error included in the absolute position of the vehicle can be effectively reduced.

FIG. 5 illustrates a more detailed control processing procedure of the animal position recording apparatus 100. The operation principle and specific configuration of this control processing procedure will be described below.
1. Principle of Operation and Outline of Action (1) GPS signals are received for a large number of moving points whose positions can be converted to the same fixed point, and the absolute position of each moving point given by the GPS signal is expressed as the position coordinates (absolute After the position is converted to the position), the random position included in the absolute position indicated by the GPS signal can be eliminated by averaging the absolute position. In particular, here, the optimal solution of the absolute position of the continuously observed inanimate (environmental feature) in the driving environment around the road is sequentially recalculated in real time.

(2) The detailed processing method can be recursively configured by using, for example, an extended Kalman filter. In particular, in this case, the absolute position of the host vehicle and its surrounding animals can be estimated simply by sequentially executing Markov chain information processing, so that the amount of information to be temporarily stored in the primary storage device 150 is reduced. It can be suppressed very effectively.

2. Specific Processing Procedure Hereinafter, the processing procedure of the animal position recording apparatus 100 will be described in detail with reference to FIG.
In the first step 1100 of the control loop, the absolute positions of the environmental features classified as inanimate and their positioning accuracy are predicted. This predicted value can be obtained from the absolute position estimated in the latest past (previous control cycle). However, since the non-animal environmental feature is selected here, the predicted value (the absolute position in the current control cycle) of the current environmental feature predicted in step 1100 does not basically change.

In the next step 1110, the absolute position of the host vehicle and its positioning accuracy are predicted. This predicted value can be obtained from the absolute position of the inanimate object or the own vehicle, the estimated value of the motion of the own vehicle, the estimation accuracy related to these, etc., estimated in the most recent past (previous control cycle).
However, you may consider the movement of the own vehicle for the past several times. The length of such observation period that should be conscious of these sequential calculation processes can be set arbitrarily. Of course, the observation data acquired this time (including the probability) and the past observation data for one time It is also possible to construct the algorithm recursively (Markov chain) so that only estimated data is handled.

In step 1200, it is confirmed whether or not new environmental features around the host vehicle have been acquired by the environmental feature acquisition device 120 of FIG. 2 that is operating asynchronously.
In step 1210, features belonging to a stationary object are extracted from the acquired environmental features, and their relative positions and probabilities are calculated. For this stationary object extraction process, a discrimination means is useful. However, these processes may be executed asynchronously in advance in the selection / integration / analysis unit 128 of FIG.
Then, in the next step 1220, the current movement of the host vehicle (speed vector, yaw angular velocity, etc.) and its certainty are estimated by obtaining the time change of the relative position from the previous cycle.

In step 1300 / step 1400, the presence / absence of reception of a GPS signal is confirmed. In step 1310/1410, the absolute position of the host vehicle given by the GPS signal and its certainty are calculated.
In step 1320, from the absolute position of the own vehicle acquired using the GPS receiver (own vehicle position acquisition device 110) and the relative position of the environmental feature acquired using the positioning means (environment feature acquisition device 120) with respect to the own vehicle. The absolute position of the environmental feature and its certainty are calculated. However, if a new GPS signal is not obtained, in step 1330, the predicted values of the absolute position and the likelihood of the environmental feature predicted based on the absolute position of the host vehicle in the past are substituted.

  After that, in step 1500, it is confirmed whether or not the absolute position information of the inanimate being measured by the current positioning device (environment feature acquisition device 120) already exists in the environmental feature storage device 160, and the corresponding data exists. If so, the process goes to Step 1510; otherwise, the process goes to Step 1520. In this search process, the search collation means provided in the inanimate position evaluation unit 132 is used.

  In the next step 1510, the corresponding data on the hit environmental feature storage device 160, the predicted value obtained in step 1100 (the absolute position of the environmental feature and its probability), and the environmental feature given in step 1320 or step 1330 are displayed. Based on the absolute position and the certainty, the absolute position and the certainty of the environmental feature observed again in the current control cycle are updated after taking these certainties into account. This process superimposes the absolute position / accuracy of the observed environmental feature and the absolute position / accuracy of the environmental feature predicted from the past, thereby averaging the desired absolute position, thereby reducing error variance. .

  On the other hand, in step 1520, based on the predicted value obtained in step 1100 (the absolute position of the environmental feature and its certainty) and the absolute position of the environmental feature given in step 1320 or step 1330 and its certainty, Considering the uniqueness, the absolute position and the probability of the environmental feature observed again in the current control cycle are updated. This processing also superimposes the absolute position / accuracy of the observed environmental feature and the absolute position / accuracy of the environmental feature predicted from the past, thereby averaging the desired absolute position, thereby reducing error variance. Become.

  Thereafter, in step 1600, the absolute position and the accuracy of the host vehicle are calculated from the updated absolute position and accuracy of the environmental feature and the accuracy of the surrounding monitoring sensor (environment feature acquisition device 120). As described above, the absolute position of the host vehicle can be obtained with higher accuracy than in the past by the estimation process that effectively uses the measurement result obtained by the positioning device and the GPS signal.

  On the other hand, if it is determined in step 1400 that the observation value of the environmental feature is newly obtained, in step 1610, the predicted value obtained in step 1110 is obtained only for the current absolute position of the own vehicle and its likelihood. And updating based on the observed value obtained in step 1410. Also, if it is determined in step 1400 that the observation value of the environmental feature is not newly obtained, in step 1620, only the current absolute position of the host vehicle and its likelihood are obtained in step 1110. Update based only on the predicted value. That is, in the control cycle in which new information is not obtained from any of the acquisition devices, the predicted value (the current absolute position of the host vehicle and the probability thereof) obtained in step 1110 is directly used as the estimated value in step 1620. Become.

Note that the above estimation calculation processing is performed using the primary storage device 150 in FIG. 1, and various calculation processing results such as the predicted value and the estimation value are also stored on the storage device 150.
Moreover, these estimation calculation processes can also be comprised using an extended Kalman filter etc., for example. Such mathematical formulation is performed using, for example, a covariance matrix Σ shown below, and thereby, an operation equivalent to the processing procedure of FIG. 5 can be derived.
Hereinafter, the configuration of the extended Kalman filter when a laser radar is used as the external sensor (positioning means) will be described.
(Extended Kalman filter)

Here, the position parameter x to be estimated (: state vector x to be estimated) is set as in the above equation (1), and the observed value y (measurement vector y) is set in accordance with the above equation (2). However, here, the north direction in the north-south direction is the positive direction in the Z-axis direction, the east direction in the east-west direction is the positive direction in the X-axis direction, and the absolute position of the vehicle in this absolute coordinate system is (X _vo , Z _vo ). Further, the direction (azimuth angle) of the host vehicle with respect to the Z axis is φ _v , the host vehicle velocity vector is V _v , the host vehicle yaw angular velocity is ω _v, and the absolute position of the i-th animal is (X _fi , Z _fi ). The position parameter x in equation (1) is composed of these variables.

In addition, the observed value y observed for estimating such a position parameter x is composed of the following variables. That is, the absolute position (X _g , Z _g ) of the own vehicle given by the GPS signal, the velocity vector V _{g of the} own vehicle given by the GPS signal, and the relative position of the i th environmental feature measured by the laser radar with respect to the own vehicle. (R _sfi , φ _sfi ). Here, r _sfi is a distance from the own vehicle to the i-th environmental feature, and φ _sfi is an azimuth angle of the i-th environmental feature measured from a relative coordinate axis fixed in the front direction of the own vehicle. The observation value y in equation (2) consists of these variables.
Then, by periodically repeating the following (procedure a) to (procedure c), the position parameter x can be calculated sequentially in real time.

(Calculation procedure)
(Procedure a) The current position (t = k) and variance are predicted from the position parameters estimated at the previous time (t = k-1) and their variances. This procedure a corresponds to the above equation (3) for predicting the position parameter x and the above equation (4) for predicting the covariance matrix Σ. The procedure a corresponds to step 1100 and step 1110 in FIG.

(Procedure b) The Kalman gain is calculated based on the prediction error (Σ _{k / k−1} ) and the observation noise calculated in the procedure a. This procedure b corresponds to the above equation (5) for calculating the Kalman gain K.

(Procedure c) Based on the calculated Kalman gain, the observed value and the predicted value are integrated, and the current position parameters and their variances are estimated. The procedure c corresponds to the above equation (6) for estimating the position parameter x and the above equation (7) for estimating the covariance matrix Σ. Further, this procedure c and the above procedure b correspond to step 1600, step 1610, step 1620, etc. in FIG.

  The above calculation procedure formulated using the extended Kalman filter leads to a calculation result equivalent to the processing procedure of FIG. In other words, the processing procedure of FIG. 5 can be configured to derive an operation result equivalent to such an operation procedure expressed using an extended Kalman filter.

  According to the above processing procedure, from the position parameter x and the covariance matrix Σ, etc., the current absolute position and motion of the host vehicle (velocity vector and yaw angular velocity), the absolute position of the focused inanimate object, Each positioning accuracy and the like can be obtained with high accuracy in real time while moving with the host vehicle, and at the same time, these positioning data can be recorded in the environmental feature storage device 160 at any time by step 1700 in FIG. .

Of particular note here is the fact that specific landmarks such as intersections, utility poles, reflectors, road signs, road surface displays and the like that have been frequently used in the past may not exist around the host vehicle. 2, that is, various environmental characteristics illustrated in FIG. 2 are almost certainly present in the observation signals from the external sensors (121 to 124). Furthermore, there are a great many of these environmental features in the observed signals obtained by one sensing. For this reason, if the above-described animal position recording apparatus 100 is mounted on the vehicle, the position of the surrounding animal and the own vehicle can be estimated with high accuracy without being influenced by the road environment.
At the same time, if the number of feature points of inanimate animals and their position information are sufficiently obtained, the estimation accuracy of the movement of the own vehicle will also improve. The absolute position can be estimated.

FIG. 6 illustrates an effect (simulation result) based on the above-described animal position recording apparatus 100. This graph shows the relationship between the time taken to observe environmental features using the positioning device (environmental feature acquisition device 120) and the positioning accuracy of the absolute position in the traveling direction of the host vehicle. The obtained positioning accuracy was indicated by a value (2σ) twice the square root of the error variance.
In this simulation, it was assumed that there was no building high enough to prevent reception of GPS signals from GPS satellites, and the positioning accuracy of GPS alone was assumed to be 2σ = 5 m. Further, it was assumed that a laser radar was used as a sensor for detecting the environmental feature, and thereby the relative position of the fixed points of 2 to 4 points could always be detected within an error range of 2σ = 50 mm. In addition, the GPS signal reception cycle in this simulation was 1 time / second, and the observation period by the laser radar was 20 times / second. In addition, assuming that the traveling speed of the vehicle is 40 km / h, the distance range that can be measured by the laser radar is 20 m to 70 m forward, and the angle range that can be measured is 40 ° front left to right front with the front in front being 0 °. The angle was 40 °. In order to realize (simulate) the arithmetic processing illustrated in the flowchart of FIG. 5 in more detail, a recursive algorithm substantially equivalent to a general extended Kalman filter corresponding to these arithmetic processing is used.
From such a simulation result, when the present inanimate position recording device 100 is used, the positioning accuracy of the absolute position of the inanimate animal accumulated in the environmental feature storage device 160 is significantly higher than the conventional case. You can see that In addition, the present animal position recording apparatus 100 is very excellent in that the absolute position information of an animal having an error of about 1 m can be secured for a sufficient number of points while traveling with the host vehicle. I can say.

FIG. 7 shows a logical structure of the host vehicle position estimation apparatus 200 used by the end user. The own vehicle position estimating device 200 can be configured by expanding and deforming the inanimal position recording device 100 of the first embodiment, and the portions having the substantially equivalent functions / operations are described above. The same symbol is attached. For example, instead of outputting the positioning data of the host vehicle and the inanimate animals to the environmental feature storage device 160, the host vehicle position evaluation unit 132 ′ can accurately detect the current absolute position of the host vehicle with respect to any other application. Except for the point of outputting the estimated value, it has substantially the same function / operation as the moving object position evaluation unit 132 of FIG.
In addition, in the environmental feature storage device 160 in FIG. 7, a map database that holds position data of several environmental features (animals) whose absolute positions are already known is already constructed in advance.

For this reason, according to this own vehicle position estimation device 200, not only can random noise included in the absolute position given by the GPS signal be effectively eliminated, but also offset errors included in those absolute positions can be reduced or eliminated. It becomes possible to do.
That is, the largest feature in the various operations of the own vehicle position estimating device 200 is the environmental feature acquired by the environmental feature acquiring device 120 and the map database (that is, the environmental feature information 160A in FIG. 4) that the environmental feature storage device 160 has. The environmental feature corresponding to the known absolute position is collated by the environmental feature correspondence evaluation unit 270 having means corresponding to search collation means, and the absolute position of the host vehicle is detected or corrected based on the collation result. In the point.

  Therefore, according to the own vehicle position estimation apparatus 200 of FIG. 7 having the laser radar 122 of FIG. 2, for example, a range profile as shown in FIG. By checking the corresponding data stored in advance in the storage device 160 in real time, it is possible to detect the absolute position of the host vehicle with high accuracy in real time even on a road where the vehicle travels for the first time or even when traveling at high speed. It becomes possible.

  In the device 200 as well, since the above-described identification processing (discriminating means) included in the vehicle motion estimation unit 140 is capable of high-accuracy estimation of the own vehicle motion as in the previous animal position recording device 100, As long as the absolute position of an environmental feature and its certainty are registered in the environmental feature storage device 160 at relatively long distance intervals, the end user corrects the absolute position of the vehicle. It is possible to perform accurately.

For this reason, if this own vehicle position estimating device 200 is used, it is possible to obtain high accuracy and high reliability with respect to the absolute position of the own vehicle even when traveling at a relatively high speed.
The environmental feature storage device 160 only needs to register the absolute position and certainty of some environmental feature at a relatively long distance interval as described above. The production cost of the environmental feature storage device 160) and the in-vehicle cost can also be kept much lower than before.

  It should be noted that various calculation results such as the environmental features and the motion of the host vehicle obtained by the inanimate position estimation unit 130 and the host vehicle motion estimation unit 140 in FIG. 1 are stored in the environment feature storage device 160 according to their certainty. It is more desirable to make a determination as to whether or not registration is necessary in order to make desired positioning data highly accurate. In the second embodiment, such a database update method will be exemplified.

FIG. 8 shows a specific update mode for the environmental feature storage device 160 (FIG. 1) in the second embodiment. For example, the processing step group 1700 ′ in FIG. 8 corresponds to step 1700 in FIG. Is. Further, step 2000 in FIG. 8 corresponds to step 1510 or step 1520 in FIG. 5. As one of the calculation results of step 2000, the accuracy of the current absolute position of the host vehicle (standard deviation σ _c ) Is obtained. Further, step 3000 in FIG. 8 corresponds to step 1600 or step 1610 in FIG. 5. As one of the calculation results of step 3000, the accuracy of the absolute position of the noticed environmental feature (fixed point) ( A standard deviation σ _t ) is obtained.

Hereinafter, a specific operation of the processing step group 1700 ′ will be described.
First, in step 2100, it is checked whether or not the environmental feature data DB1 stored in the environmental feature storage device 160 has an environmental feature under observation that is currently focused on. If it is registered in the data DB 1, the process proceeds to step 2200, and if not, the process proceeds to step 2400.

In step 2400, the information belongs to the stationary object that has been evaluated by the environmental feature evaluation unit 141 (such as the position and accuracy), certain conditions required for registration (e.g., a standard deviation sigma _t in FIG threshold Th _d smaller, etc.) In the subsequent step 2500, this information (position, accuracy, etc.) is registered as environmental feature data in the environmental feature data DB1.

In step 2200, it is determined whether or not a certain condition necessary for updating (for example, the standard deviation σ _t in the figure is smaller than the standard deviation σ _{d of the} registered environmental feature) is satisfied. If true, the registration information (environment feature data) already registered in the environment feature data DB 1 is updated in the subsequent step 2300.

Further, it is determined whether or not a certain condition (for example, the standard deviation σ _c is smaller than the threshold value Th _c ) holds for the traveling position of the host vehicle (step 3100). The position and speed of the host vehicle, and their probabilities are registered in the travel locus data DB 2 (step 3200). The travel route of the host vehicle may naturally vary depending on each travel time (each actual measurement date and time), so it is not necessary to update the travel route, and is registered in step 3200 each time, thereby accumulating a plurality of travel trajectories in the travel trajectory data DB2.
By accumulating a lot of such travel locus data, for example, when a plurality of lanes are provided on the road on which the vehicle has traveled, what number of lanes the vehicle has selected at that time, and the lane of the road It is also possible to estimate numbers and road widths.

  The registered information may be stored individually or may be handled on the same database. In addition, for data in which a mutual relationship is important, such as an environmental feature that can be detected at a point where the host vehicle is located, a separate correlation database (DB3) may be prepared.

According to the database update method as described above, information such as the position and attribute of the evaluated environmental feature is retained multiple times, and information such as the position and attribute of the environmental feature is based on the information obtained from the multiple times. Therefore, it is possible to effectively ensure the accuracy and reliability of the registration information of the desired database.
In addition, by referring to the certainty about the environmental characteristics already registered in the database in advance before updating the data, it is possible to prevent an update process in which the certainty of the desired environmental characteristics is lower than the current level. In addition, the accuracy and reliability of registration information in a desired database can be ensured.

[Other variations]
The embodiment of the present invention is not limited to the above-described embodiment, and other modifications as exemplified below may be made. The effects of the present invention can also be obtained based on the operation of the present invention by such modifications, expansions, or applications.

For example, as shown in the first embodiment, a predicted value such as the absolute position of the host vehicle or its surrounding inanimate animals may be estimated from the past several movements as needed. As exemplified, the motion of the host vehicle may be predicted based on a motion model set in advance.
In addition, the position / accuracy of the own vehicle and the environmental features may be updated sequentially using a Kalman filter, a particle filter, or the like, or an optimum solution is obtained using past observation values for a certain period of time. Alternatively, the estimated values estimated sequentially may be updated again in units of all databases.

  Note that the environmental feature evaluation unit 141 in FIG. 1 also includes means for identifying whether an object having environmental features is a stationary object or a moving object (that is, a determination means). In order to accurately estimate the motion of the host vehicle, it is effective to continue to observe the positions of surrounding stationary objects. In such a discrimination means, for example, an assumption that most of the objects detected by the positioning device are inanimate may be used. For this identification process, observation data one hour before stored in the primary storage device 150 is used. Then, by using only information belonging to a stationary object separated based on such an identification method, the motion estimation accuracy of the host vehicle can be further improved.

In addition, if the discriminating means is used, the movement of the moving object can be simultaneously enhanced based on the relative relationship between the moving object and the own vehicle, the relative relationship between the stationary object and the own vehicle, the absolute position and movement of the own vehicle. It can also be estimated to accuracy.
Therefore, for example, if the relative position or relative motion of the preceding vehicle with respect to the host vehicle can be accurately grasped, and the motion parameters (position, speed, estimation accuracy, etc.) of the preceding vehicle can be calculated simultaneously. It is also possible to simultaneously estimate the positions and movements of the preceding vehicle and the host vehicle using an extended Kalman filter or the like. Further, at this time, if the detection accuracy of the surrounding monitoring sensor and the motion prediction of the moving object (preceding vehicle) based on the detection accuracy are appropriate, the motion estimation accuracy of the own vehicle can be further improved. If the motion estimation method of the host vehicle is introduced, the accuracy of the absolute position information of the non-animal to be stored in the environmental feature storage device 160 can be further improved.

  INDUSTRIAL APPLICABILITY The present invention can be used when collecting absolute position information of inanimate animals around a road, which is useful for various applications for moving objects such as a ground navigation system and an in-vehicle auto cruise control system. it can. In addition, these moving bodies are not limited to four-wheeled vehicles, and of course, the present invention can be used through the various applications described above even in robots and two-wheeled vehicles.

Control block diagram showing logical structure of animal position recording apparatus 100 of embodiment 1 Data flow diagram illustrating a configuration example of the environmental feature acquisition device 120 A photograph illustrating a surveying scene by the animal position recording device 100 An example of data that can be stored in the environmental feature storage device 160 Control block related diagram illustrating re-evaluation processing procedure of environmental feature data The flowchart which illustrates the control processing procedure of the animal position recording device 100 The graph which illustrates the effect based on the animal position recording device 100 Control block related diagram showing logical structure of own vehicle position estimating apparatus 200 The flowchart which illustrates the specific update form of the environmental feature storage device 160

100: Animal-free position recording device
110: Own vehicle position acquisition device (GPS receiver)
120: Environmental feature acquisition device (positioning device)
130: Inanimal position estimation unit
140: Own vehicle motion estimation unit
160: Environmental feature storage device
200: Own vehicle position estimation device
270: Environmental feature correspondence evaluation section (search collation means)

Claims (2)

An absolute position acquisition device that measures a first observation value that is the absolute position of the traveling vehicle, and a fixed point that does not change in time in the traveling environment ahead of the traveling vehicle, and extracts the fixed point And a positioning device that measures a relative observation value that is a relative position with respect to the host vehicle in real time, and a first estimation that is an absolute position of the host vehicle that is more accurate than the first observation value from the first observation value. A fixed point position recording device for mounting on a vehicle that sequentially obtains a value as time passes and calculates and records the absolute position of the fixed point,
Fixed point absolute position calculation means for obtaining a second observed value that is an absolute position of the fixed point from the first observed value measured by the absolute position acquisition device and the relative observed value measured by the positioning device. When,
A first predicted value that is the predicted absolute position of the host vehicle at the current time is obtained from the first estimated value in the past, and in the past, the absolute position is more accurate than the second observed value of the fixed point. Absolute position predicted value calculation means for obtaining a second predicted value that is an absolute position of the fixed point at the current time from a second estimated value;
The first predicted value and the second predicted value obtained by the absolute position predicted value computing means, the second observed value obtained by the fixed point absolute position computing means, and the absolute position acquisition device. Based on the first observed value, the first predicted value and the second predicted value are corrected so that an error variance of the first estimated value and the second estimated value is reduced, and the first predicted value is corrected. Absolute position correcting means for obtaining an estimated value and the second estimated value;
An environmental feature storage device for recording the second estimated value of the fixed point obtained by the absolute position correcting means and the error variance of the second estimated value as a reference absolute position and a reference error variance of the reference fixed point , respectively ;
Search collating means for collating the second estimated value of the fixed point obtained by the absolute position correcting means with the reference absolute position of the fixed point stored in the environmental feature storage device;
When the search collating unit determines that the second estimated value matches the reference absolute position, the error variance obtained by the absolute position correcting unit is smaller than the reference error variance of the reference fixed point. A fixed point position recording apparatus comprising: a data updating means for rewriting an old reference error variance with the newly obtained error variance as a new reference error variance .   The correction by the absolute position correcting means includes a first deviation of the first observed value with respect to the observed value corresponding to the first predicted value, a second deviation of the second observed value with respect to the observed value corresponding to the second predicted value, The fixed point position recording apparatus according to claim 1, wherein the fixed point position recording apparatus is performed with a correction amount proportional to the error variance.