JP2008249639A - Own position locating device, own position locating method, and own position locating program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To enhance locating precision of an own position locating device, using an existing road infrastructure such as a white line on a road and a road sign. <P>SOLUTION: A DR computing part 3 calculates a vehicle position by dead reckoning, in this own position locating device 100 mounted on a vehicle of the present invention. A pseudo-distance residual calculating part 22 calculates a pseudo-distance residual, from a pseudo-distance, and a satellite-vehicle distance based on a satellite orbit parameter. A camera 7 images the road in front of the vehicle, an image processing part 8 calculates an azimuth angle of the white line photographed in an image, a white line azimuth angle acquiring part 23 acquires the azimuth angle from white line database 9, and a white line azimuth angle residual calculating part 24 calculates an azimuth angle residual, based on the azimuth angle of the white line calculated by the image processing part 8, and the azimuth angle of the white line acquired by the white line azimuth angle acquiring part 23. An extended Kalman filter 6 inputs the pseudo-distance residual and the azimuth angle residual as observed amounts, and estimates an error of a positioning result in the DR computing part 3. The DR computing part 3 corrects the positioning result, based on the estimated error estimated by the expanded Kalman filter 6. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention provides, for example, a self-positioning device, a self-positioning method, and a self-positioning method that are mounted on a geographic data collection vehicle to create a GIS (Geographic Information System) database, or are mounted on a general vehicle and used in a navigation system. It relates to the self-location program.

  GPS (Global Positioning System) receiver prices decrease and car navigation systems (hereinafter also referred to as car navigation systems and car navigation systems) become widespread, and technology and driver driving to improve the self-localization accuracy of car navigation systems Various technologies have been developed, including assistive technologies. For example, as a technique for improving the self-localization accuracy, differential GPS (DGPS) for improving the positioning accuracy by reflecting the correction information to the pseudo distance which is one of GPS observation information, or map information is used. Examples include map matching technology that corrects the self-position and technology such as lane keeping that is performed based on the recognized white line.

  The Ministry of Land, Infrastructure, Transport and Tourism has led the development of ASV (Advanced Safety Vehicle) since 1991, and is currently in the third phase of the plan to commercialize an accident avoidance system using inter-vehicle communication. want to be. Such an advanced system requires a self-localization device with higher accuracy than the present system, but a device that can be mounted on a general passenger car is sufficient in a situation where GPS availability (Availability) is poor. At present, it is difficult to maintain accurate positioning accuracy.

  2. Description of the Related Art Conventional vehicle self-positioning devices such as car navigation applications have been developed that use GPS, inertial devices, vehicle speed pulses, and the like. In order to improve the positioning accuracy of this self-localization device, it is a discontinuous line or continuous line for displaying traffic division zones on the road (usually “white line”, so it is called “white line” for the sake of convenience below), road signs, traffic lights, buildings A method for improving the self-localization accuracy using landmarks such as these has been proposed.

For example, as a method of using the white line, the white line is photographed with a video camera mounted on the vehicle, the distance between the vehicle and the white line is calculated from the observation value obtained by processing the image, and the distance between the vehicle and the white line is self-positioned. There is a method (Patent Document 1) used for correction.
In addition, as a method of using landmarks such as buildings, a position correction method based on a result of collation between an observation value by a distance measuring sensor such as a laser radar mounted on a vehicle and a value registered in a building database (Patent Document) 2) etc. are proposed. Furthermore, a method has been devised for identifying the self position from the arrangement and positional relationship of landmarks such as buildings around the vehicle (Patent Document 3).
In addition, as a method of using road signs and signals, the sign is recognized by a camera mounted on the vehicle, the distance to the sign is detected using two cameras or radar, the coordinates of the sign obtained from the database, and detection There is a method (Patent Document 4) for correcting the self-position using the measured distance. Furthermore, a signal recognition device and means for calculating the elevation angle in the direction to the signal are calculated, the horizontal distance to the signal is calculated, and the horizontal distance to the signal is used for self-position correction (Patent Document 5). Etc. have been proposed.

However, the conventional method that uses the distance to the white line recognized by the in-vehicle camera for self-position correction does not have sufficient accuracy of the map database for self-positioning devices such as car navigation systems. Very limited. At present, the accuracy of the map database used for car navigation is effective for lane keeping that corrects the position of the vehicle on the road. Is not accurate enough to achieve. Although the accuracy of the map database is expected to improve in the future, the absolute coordinate fluctuation of about several centimeters per year is usually caused by the earth's crustal movement, and the position coordinate accuracy that can be secured in the map database is limited.
Further, among the conventional methods, Patent Document 2 has a problem that a dedicated building database needs to be constructed and a high-speed and high-precision laser radar is required.

On the other hand, in such a situation, a method for creating GIS data of roads, buildings and the like by a simple method is being developed. For example, a method of constructing a white line database (Patent Document 6) has already entered the practical application stage.
No. 2687645 JP 2006-138834 A JP-A-8-247775 JP 2000-97714 A Japanese Unexamined Patent Publication No. 7-35560 JP 2006-47291 A Fujimoto, “Map data format for car navigation KIWI”, Denso Technical Review VOL. 6 No. 1, 2001

  As a vehicle self-localization device represented by a car navigation system or the like, a standard one using a GPS, an inertial device, a vehicle speed pulse, or the like is standard. Such a self-positioning device has a problem that the positioning accuracy deteriorates in an environment where the availability of GPS positioning signals is poor such as in urban areas and tunnels. Therefore, in order to maintain a certain positioning accuracy even in an environment where GPS positioning signal availability is poor, some other auxiliary means is required.

  In view of the above, an object of the present invention is to improve the positioning accuracy of the self-positioning device using existing road infrastructure such as a white line on a road or a road sign.

  The self-positioning device of the present invention is reflected in the image based on a positioning unit that measures the self-position, a camera that captures a specific direction when the positioning unit performs positioning, and an image captured by the camera. An image azimuth portion for calculating the azimuth angle of the specific feature with respect to the reference direction by using a CPU (Central Processing Unit), and the length direction of the feature is the reference direction. An azimuth angle database that stores the azimuth angle of the feature formed with respect to, and an azimuth angle of the feature corresponding to the specific feature from the azimuth angle database based on the self-position measured by the positioning unit The difference between the azimuth angle of the database, the azimuth angle of the feature calculated by the image azimuth angle portion, and the azimuth angle of the feature acquired by the database azimuth angle portion is used as a feature azimuth angle residual using the CPU. An azimuth residual calculation unit for calculating, and a Kalman filter for estimating an error of the self-position measured by the positioning unit based on the feature azimuth residual calculated by the azimuth residual calculation unit using a CPU. The positioning unit corrects the measured self-position based on the error estimated by the Kalman filter, and uses the corrected self-position as a positioning result.

  According to the present invention, for example, the azimuth angle of the white line reflected in the image is calculated by the image azimuth portion, and the azimuth angle of the white line stored in the azimuth database by the Kalman filter and the azimuth angle of the white line calculated from the image. By performing error estimation using the azimuth residual as an observation amount, it is possible to improve self-localization accuracy.

The conventional method does not use the white line azimuth data for self-position correction (Patent Document 1), or does not use the azimuth data (as viewed from the vehicle) where the road sign exists for self-position correction. (Patent Document 4), there was room for improvement as a self-positioning device.
Therefore, the present invention pays attention to the fact that the azimuth angle is less likely to change due to the earth's crustal movement, and the azimuth angle can be detected with higher accuracy than the absolute position coordinates. Is used for self-position correction to improve the self-positioning accuracy.

Embodiment 1 FIG.
FIG. 1 is a configuration diagram of a self-positioning apparatus 100 according to the first embodiment.
FIG. 2 is a flowchart showing a self-positioning method of self-positioning apparatus 100 in the first embodiment.
The outline of the configuration of the self-positioning apparatus 100 according to the first embodiment and the outline of the self-positioning method executed by the self-positioning apparatus 100 will be described below with reference to FIGS. 1 and 2.
Each part of the self-positioning device 100 executes each process described based on FIG. 2 using the CPU.

  In FIG. 1, a self-positioning device 100 is mounted on a vehicle (an example of a moving body), and an error included in a positioning result of a positioning unit 110 measured by an arbitrary positioning method is estimated using an error estimating unit 140. The positioning result of the positioning unit 110 is corrected by the error, and the position and posture angle of the vehicle are determined with high accuracy.

<S110: Positioning process>
In FIG. 2, the positioning unit 110 calculates the vehicle position and attitude angle by dead reckoning calculation using azimuth angle data and velocity data of the vehicle and strapdown calculation using a three-axis accelerometer and a three-axis gyro. To do. In many cases, azimuth angle data used for dead reckoning calculation is obtained from a gyro, and speed data is obtained from a vehicle speed pulse.

<S120: GPS observation residual calculation processing>
Next, the GPS observation residual calculation unit 120 performs GPS observation on the difference between the observation value of the GPS observation unit 121 and the calculation value of the GPS observation amount calculation unit 122 for the GPS observation amount such as the pseudorange, the phase difference, and the Doppler amount. Calculate as residual.
At this time, the GPS observation unit 121 receives a positioning signal using a GPS receiver to acquire a GPS observation amount, and the GPS observation amount calculation unit 122 calculates an amount corresponding to the GPS observation amount of the GPS observation unit 121 by calculation. To do. Then, the GPS observation residual calculation unit 120 inputs the difference between the observation value of the GPS observation unit 121 and the calculation value of the GPS observation amount calculation unit 122 to the error estimation unit 140 as a GPS observation residual. The GPS observation amount obtained by the GPS observation unit 121 includes, for example, a GPS satellite and a GPS receiver calculated by multiplying the time from when the GPS satellite transmits a positioning signal until the GPS receiver receives the positioning signal by the speed of light. There is a pseudo-range between In addition, for example, when a positioning signal (carrier wave) from a specific GPS satellite is received by a plurality of GPS receivers, a phase difference (single phase difference) of received positioning signals between GPS receivers or a single position between GPS receivers. The double phase difference indicating the difference between the single phase differences when the phase difference is obtained for a plurality of GPS satellites is also an example of the GPS observation amount. Further, for example, a Doppler amount indicating a relative speed between a GPS satellite and a GPS receiver calculated based on a difference between the frequency of the received positioning signal and the reference frequency of the positioning signal is an example of the GPS observation amount. For example, the GPS observation amount calculation unit 122 calculates a distance corresponding to the pseudorange based on the position of the vehicle measured by the positioning unit 110 and the position of the GPS satellite indicated by the satellite orbit parameter included in the positioning signal. In addition, for example, the GPS observation amount calculation unit 122 generates a LOS (Line Of Light) vector indicating the direction from each GPS receiver to each GPS satellite and a baseline vector indicating the direction from one GPS receiver to another GPS receiver. Based on this, a phase difference and a double phase difference are calculated. Further, for example, the GPS observation amount calculation unit 122 calculates the Doppler amount based on the position / velocity of the GPS satellite and the position / velocity of the vehicle. Then, the GPS observation residual calculation unit 120 calculates the difference between the GPS observation value of the GPS observation unit 121 and the calculation value corresponding to the GPS observation value of the GPS observation amount calculation unit 122 as the GPS observation residual.

<S130: Azimuth residual calculation processing>
Next, the azimuth residual calculation unit 130 calculates the azimuth and observation values of the azimuth observation unit 131 for the azimuth of the straight line portion of the white line on the road on which the vehicle is traveling and the azimuth of the sign viewed from the vehicle. The difference from the calculated value of the unit 132 is calculated as the azimuth residual.
At this time, the azimuth angle observation unit 131 captures the traveling direction of the vehicle using a camera and acquires the observation value of the azimuth angle of the straight line portion of the white line and the azimuth angle of the sign viewed from the vehicle. Based on the data registered in the database, the calculated values of the white line azimuth and the sign azimuth are obtained. Then, the azimuth residual calculation unit 130 inputs the difference between the observation value of the azimuth observation unit 131 and the calculation value of the azimuth calculation unit 132 to the error estimation unit 140 as an azimuth residual.

<S140: Error Estimation Processing / Correction Processing>
Then, the error estimation unit 140 estimates an error included in the positioning result of the positioning unit 110 based on the GPS observation residual and the azimuth residual.
The positioning unit 110 corrects the position and the posture angle based on the error estimated by the error estimation unit 140.
At this time, the error estimator 140 performs position and orientation angle error estimation using a Kalman filter.
The Kalman filter performs state quantity error estimation based on a state equation modeling the state quantity dynamics and an observation equation that formulates the relationship between the observed quantity and the state quantity. When locating the self-location, the Kalman filter uses the difference between the observed value of the specific observed value by the GPS receiver and the calculated value (theoretical value) corresponding to the specific observed value (observed residual) as the observed value. Then, error estimation is performed for state quantities such as the coordinates of the self position and the posture angle (azimuth angle, elevation angle, rotation angle). The Kalman filter calculates the coordinates of the self position and the error of the posture angle as a state quantity.

However, when a vehicle equipped with the self-positioning device 100 cannot receive a positioning signal from a GPS satellite when moving in a tunnel or between buildings, the error estimation unit 140 (Kalman filter) may perform error estimation. The positioning accuracy of the self-positioning device 100 is deteriorated.
Therefore, the error estimation unit 140 (Kalman filter) according to the first embodiment uses the azimuth residual relating to the white line and the sign that can be observed based on the image even when the positioning signal cannot be received as the observation amount, and the positioning result of the positioning unit 110. Perform error estimation.

  Here, a method using the distance between the vehicle and the white line or the distance between the vehicle and the sign as an observation amount other than the GPS observation amount is also conceivable, but the method using the distance as the observation amount is a stereo in order to obtain an observation value of the distance. There is a problem that a camera or a laser radar is required, or that the accuracy of the distance calculation value calculated based on the position of the white line or the sign registered in the database is lowered due to a decrease in the accuracy of the database due to crustal movement.

  On the other hand, when the azimuth angle is used as the observation amount other than the GPS observation amount as in the first embodiment, if there is one monocular camera, the observation value of the azimuth angle can be obtained from the image captured by the camera. There is an advantage that the accuracy of the calculated value of the azimuth based on the position of the white line and the sign stored in the database is relatively high because the angle is less changed due to crustal movement.

  In addition to the error in the positioning result of the positioning unit 110, the error estimation unit 140 (Kalman filter) also includes a rate error for the gyro used by the positioning unit 110 and a clock error and direction for the GPS receiver used by the GPS observation residual calculation unit 120. Error estimation is also performed for the alignment error of the camera used by the angular residual calculation unit 130, and the estimated error is output for correction.

FIG. 3 is a diagram illustrating an example of hardware resources of the self-positioning apparatus 100 according to the first embodiment.
In FIG. 3, the self-positioning apparatus 100 includes a CPU 911 (also referred to as a central processing unit, a central processing unit, a processing unit, an arithmetic unit, a microprocessor, a microcomputer, and a processor) that executes a program. The CPU 911 is connected to the ROM 913, the RAM 914, the communication board 915, the display device 901, the GPS receiver 1, the gyro 4, the odometer 5, the camera 7, and the magnetic disk device 920 via the bus 912, and controls these hardware devices. . Instead of the magnetic disk device 920, a storage device such as an optical disk device or a memory card read / write device may be used.
The RAM 914 is an example of a volatile memory. The storage medium of the ROM 913 and the magnetic disk device 920 is an example of a nonvolatile memory. These are examples of a storage device, a storage device, or a storage unit. A storage device in which input data is stored is an example of an input device, an input device, or an input unit, and a storage device in which output data is stored is an example of an output device, an output device, or an output unit.
The communication board 915 is an example of an input / output device, an input / output device, or an input / output unit.

  The communication board 915 is connected to a communication network such as a LAN (local area network), the Internet, a WAN (wide area network), and a telephone line by wireless or wired.

The gyro 4 is a sensor that measures angular acceleration (hereinafter referred to as a rate) of an azimuth angle, an elevation angle, and a rotation angle of a vehicle on which the self-positioning device 100 is mounted. The gyro 4 may be a single-axis gyro that measures only the azimuth rate as will be described later.
The odometer 5 is a vehicle speed sensor that outputs a pulse (hereinafter referred to as a vehicle speed or a vehicle speed pulse) in accordance with the vehicle speed of the vehicle on which the self-positioning device 100 is mounted.
The camera 7 is installed toward the front of the vehicle on which the self-positioning device 100 is mounted, and images white lines and road signs positioned in the traveling direction of the vehicle. The camera 7 may be a monocular camera. However, a stereo camera may be used. In addition to the camera 7, a laser radar may be provided to measure the distance to a feature such as a white line or a sign.
The GPS receiver 1 receives a positioning signal from a GPS satellite, calculates a pseudo distance based on a time from when the GPS satellite transmits a positioning signal to receiving the positioning signal, or calculates the pseudo distance or the received positioning signal. The self-position is calculated based on the phase. One GPS receiver 1 is provided in the vehicle. When observing a double phase difference as a GPS observation amount, another GPS receiver is provided at a point outside the vehicle where the position coordinates are known, and the observation data of the GPS receiver is transmitted to the vehicle for use. There is also a case.

  The magnetic disk device 920 stores an OS 921 (operating system), a program group 923, and a file group 924. The programs in the program group 923 are executed by the CPU 911 and the OS 921.

  The program group 923 stores a program for executing a function described as “˜unit” in the embodiment. The program is read and executed by the CPU 911.

In the file group 924, in the embodiment, result data such as “determination result”, “calculation result of”, “processing result of” when executing the function of “to part”, “to part” The data to be passed between programs that execute the function “,” other information, data, signal values, variable values, and parameters are stored as items “˜file” and “˜database”. Self-position / attitude angle measured by positioning unit 110, image captured by camera 7, white line information and sign information used for obtaining azimuth by azimuth residual calculation unit 130, GPS observed by GPS observation residual calculation unit 120 Amount of observation, GPS observation residual calculated by the GPS observation residual calculation unit 120, azimuth residual of the white line / signs obtained from the image by the azimuth residual calculation unit 130, and azimuth residual calculated by the azimuth residual calculation unit 130 The difference is an example of what is included in the file group 924.
The “˜file” and “˜database” are stored in a recording medium such as a disk or a memory. Information, data, signal values, variable values, and parameters stored in a storage medium such as a disk or memory are read out to the main memory or cache memory by the CPU 911 via a read / write circuit, and extracted, searched, referenced, compared, and calculated. Used for CPU operations such as calculation, processing, output, printing, and display. Information, data, signal values, variable values, and parameters are temporarily stored in the main memory, cache memory, and buffer memory during the CPU operations of extraction, search, reference, comparison, operation, calculation, processing, output, printing, and display. Is remembered.
In addition, arrows in the flowcharts described in the embodiments mainly indicate input / output of data and signals. The data and signal values are the RAM 914 memory, the FDD 904 flexible disk, the CDD 905 compact disk, and the magnetic disk device 920 magnetic field. It is recorded on a recording medium such as a disc, other optical discs, mini discs, DVD (Digital Versatile Disc). Data and signal values are transmitted online via a bus 912, signal lines, cables, or other transmission media.

  In addition, what is described as “˜unit” in the embodiment may be “˜circuit”, “˜device”, “˜device”, and “˜step”, “˜procedure”, “˜”. Processing ". That is, what is described as “˜unit” may be realized by firmware stored in the ROM 913. Alternatively, it may be implemented only by software, or only by hardware such as elements, devices, substrates, and wirings, by a combination of software and hardware, or by a combination of firmware. Firmware and software are stored as programs in a recording medium such as a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, and a DVD. The program is read by the CPU 911 and executed by the CPU 911. In other words, the self-location determination program causes the computer to function as “to part”. Alternatively, the procedure or method of “to part” is executed by a computer.

FIG. 4 is a configuration diagram of the self-positioning apparatus 100 according to the first embodiment.
As a specific example of the self-positioning apparatus 100 described with reference to FIG. 1, the positioning unit 110 performs positioning by dead reckoning (dead reckoning) using the gyro 4 and the odometer 5, and the GPS observation residual calculation unit 120. Calculates the pseudorange residual using the pseudorange as the GPS observation amount, the azimuth residual calculation unit 130 calculates the azimuth residual of the white line, and the error estimation unit 140 calculates the pseudorange residual and the white line azimuth residual. The difference is input as an observation amount, and the navigation error included in the positioning result of the positioning unit 110 and the alignment error of the camera 7 used by the azimuth residual calculation unit 130 for imaging the white line are calculated as state quantities, and the correction amount Will be described below with reference to FIG.

In FIG. 4, the self-positioning device 100 includes a DR calculation unit 3, a gyro 4, and an odometer 5 as a positioning unit 110.
The gyro 4 is a uniaxial gyro that measures the rate of the azimuth angle of a vehicle on which the self-positioning device 100 is mounted.
The odometer 5 outputs the vehicle speed of the vehicle on which the self-positioning device 100 is mounted.
The DR calculation unit 3 estimates the amount of movement of the vehicle based on the azimuth rate output from the gyro 4 and the vehicle speed output from the odometer 5, and reflects the estimated amount of movement in the previous positioning result to reflect the position of the vehicle. The dead reckoning for calculating the azimuth angle is executed using the CPU.

The self-localization device 100 includes a GPS receiver 1, a satellite orbit calculation unit 2, a satellite distance calculation unit 21, and a pseudorange residual calculation unit 22 as a GPS observation residual calculation unit 120. The calculation unit 2 constitutes a GPS observation unit 121, and the satellite distance calculation unit 21 constitutes a GPS observation amount calculation unit 122.
The GPS receiver 1 receives a positioning signal transmitted from a GPS satellite, and calculates a pseudo distance between the vehicle and the GPS satellite based on a time from when the GPS satellite transmits a positioning signal until the positioning signal is received. Further, the GPS receiver 1 outputs a satellite orbit parameter indicating the orbit of the GPS satellite included in the received positioning signal.
The satellite orbit calculation unit 2 calculates the position of the GPS satellite based on the satellite orbit parameters output from the GPS receiver 1 using the CPU.
The satellite distance calculation unit 21 calculates the distance between the vehicle and the GPS satellite using the CPU as the satellite distance based on the position of the vehicle calculated by the DR calculation unit 3 and the position of the GPS satellite calculated by the satellite orbit calculation unit 2. To do.
The pseudorange residual calculation unit 22 calculates the difference between the pseudorange calculated by the GPS receiver 1 and the satellite distance calculated by the satellite distance calculation unit 21 using the CPU as a pseudorange residual.

The self-localization device 100 includes a camera 7, an image processing unit 8, a white line information database 9, a white line azimuth angle acquisition unit 23, and a white line azimuth angle residual calculation unit 24 as an azimuth residual calculation unit 130. The image processing unit 8 constitutes an azimuth angle observation unit 131, and the white line information database 9 and the white line azimuth angle acquisition unit 23 constitute an azimuth angle calculation unit 132.
The camera 7 is a monocular camera installed forward with respect to the vehicle, and images the traveling direction of the road on which the vehicle is traveling.
The image processing unit 8 (an example of an image azimuth angle unit) is a white line reflected in the image based on the angle formed by the white line reflected in the image captured by the camera 7 and the azimuth angle of the vehicle calculated by the DR calculation unit 3. The azimuth angle of is calculated.
The white line information database 9 (an example of an azimuth angle database and a position database) stores in advance information such as position (coordinates, lanes, etc.), shape (straight lines, curves, etc.), azimuth, etc., for each white line. For example, the azimuth angle information indicates the azimuth in which the downstream side (travel destination) of the white line is directed from the upstream side of the white line as a base point according to the direction of travel of the lane where the white line is located. FIG. 10 shows the relationship between the azimuth angle of the white line and the white line (the straight line portion thereof) and the lane traffic direction. The white line information database 9 can be constructed by using, for example, Patent Document 6. The database created according to Patent Document 6 is composed of three types of straight line / circular curve / clothoid for plane alignment and two types of straight line / quadratic curve for longitudinal curves.
The white line azimuth angle acquisition unit 23 (database azimuth angle unit) acquires the azimuth angle of the white line located in the traveling direction of the vehicle from the white line information database 9 based on the vehicle position and azimuth angle calculated by the DR calculation unit 3. .
The white line azimuth residual calculation unit 24 calculates the difference between the white line azimuth based on the image output from the image processing unit 8 and the white line azimuth based on the white line information database 9 output from the white line azimuth acquisition unit 23. The angle residual is calculated using the CPU.

In addition, the self-location determining apparatus 100 includes the extended Kalman filter 6 as the error estimation unit 140.
The extended Kalman filter 6 inputs the pseudorange residual calculated by the pseudorange residual calculation unit 22 and the white line azimuth residual calculated by the white line azimuth residual calculation unit 24 as an observation amount, and the deadline of the DR calculation unit 3 The navigation error included in the positioning result by reconning and the alignment error of the camera 7 are calculated as state quantities and output as correction quantities.

FIG. 5 is a flowchart showing the self-positioning method 1 of the self-positioning device 100 according to the first embodiment.
The self-positioning method 1 of the self-positioning device 100 shown in FIG. 4 will be described below based on FIG.
5, S210 corresponds to the positioning process (S110) of FIG. 2, S221 to S224 correspond to the GPS residual calculation process (S120) of FIG. 2, and S231 to S234 are the azimuth residual calculation process of FIG. Corresponding to (S130), S240 corresponds to the error estimation processing (S140) of FIG.
Each unit of the self-positioning apparatus 100 executes each process described below using a CPU.

<S210: Example of positioning process>
First, the DR calculation unit 3 inputs the vehicle azimuth rate from the gyro 4 and the vehicle speed from the odometer 5 as needed. Then, the DR operation unit 3 integrates the azimuth rate and the vehicle speed to calculate the amount of movement from the previous positioning, and adds the amount of movement from the previous positioning to the position and azimuth of the vehicle measured last time. Then, dead reckoning processing (or inertial navigation processing) for calculating the current position and azimuth of the vehicle is performed. For example, the DR calculation unit 3 calculates the azimuth angle of the vehicle with east, west, south, and north as the reference direction.
The DR operation unit 3 corrects the calculated vehicle position and azimuth based on the navigation error estimated by the extended Kalman filter 6, and outputs the corrected values as the vehicle position and azimuth.

<S221>
Further, the GPS receiver 1 receives GPS signals from the GPS receiver 1 (vehicle) when the DR calculation unit 3 performs positioning based on the positioning signal received from the GPS satellite when the DR calculation unit 3 performs positioning. The pseudo distance to the satellite and the satellite orbit parameters indicated by the positioning signal are output.
<S222>
Next, the satellite orbit calculation unit 2 calculates the position of the GPS satellite based on the satellite orbit parameters output from the GPS receiver 1.
<S223>
Next, the satellite distance calculation unit 21 calculates the distance from the vehicle to the GPS satellite as the satellite distance based on the position of the vehicle measured by the DR calculation unit 3 and the position of the GPS satellite calculated by the satellite orbit calculation unit 2.
<S224>
Next, the pseudorange residual calculation unit 22 calculates a difference between the pseudorange calculated by the GPS receiver 1 and the satellite distance calculated by the satellite distance calculation unit 21 as a pseudorange residual.

<S231: Example of Imaging Process>
Further, when the DR calculation unit 3 performs positioning, the camera 7 images the traveling direction of the road on which the vehicle is traveling.
<S232: Example of Image Azimuth Calculation Processing>
Next, the image processing unit 8 detects a white line on the road shown in the image from the image captured by the camera 7. For example, the image processing unit 8 binarizes the image and extracts feature points such as edge portions, and detects a white line on the image based on the shape formed by each feature point and the position of the feature point (whether or not it is on the road surface) To do. For example, the image processing unit 8 detects a white line based on the color information of the image.
Then, the image processing unit 8 calculates the angle formed by the length direction (straight line portion) of the white line on the image (the relative angle of the white line with respect to the vehicle), and the white line on the image is set at the azimuth angle of the vehicle calculated by the DR calculation unit 3. Are added to calculate the azimuth angle of the white line. Alternatively, there is a method of directly calculating the absolute azimuth angle of the white line.
For the white line projected as an image on the two-dimensional imaging surface according to the characteristics of the camera 7, the image processing unit 8 determines the angle of the white line on the image according to the characteristics of the camera 7 and the three-dimensional angle in the real world. The relative angle and azimuth angle of the actual white line are calculated by converting to.
For example, the information on the visual line angle of a plurality of points on the white line obtained from the image of the white line, the height information from the road surface of the camera, the information on the shape of the road, the vehicle position calculated by the DR calculation unit 3, and The azimuth angle of the white line can be calculated by calculating the position coordinates of a plurality of points on the white line using the camera position coordinates and orientation information calculated from the azimuth angle.
Here, it is assumed that the camera 7 is attached so that the imaging direction is the same as the traveling direction of the vehicle. However, when the camera 7 is installed in the vehicle, it may be possible that the installation angle of the camera 7 is slightly deviated from the vehicle. Therefore, the image processing unit 8 adds the camera alignment error estimated by the extended Kalman filter 6. It is also possible to calculate the azimuth angle of the white line. In that case, when estimating the camera alignment error, it is necessary to calculate the angle by detecting both the left and right white lines of the vehicle.
<S233: Example of database azimuth angle acquisition process>
Next, the white line azimuth angle acquisition unit 23 acquires from the white line information database 9 the azimuth angle of the white line located in the traveling direction of the vehicle based on the vehicle position and azimuth angle measured by the DR calculation unit 3. That is, the white line azimuth angle acquisition unit 23 acquires the azimuth angle from the white line information database 9 for the white line reflected in the image captured by the camera 7. For example, the white line information database 9 indicates the azimuth angle of the white line with east, west, south, and north as reference orientations.
<S234: Example of azimuth residual calculation process>
Next, the white line azimuth residual calculation unit 24 calculates the difference between the white line azimuth calculated by the image processing unit 8 based on the image and the white line azimuth obtained by the white line azimuth acquisition unit 23 from the white line information database 9. Calculated as white line azimuth residual.

<S240: Example of Error Estimation Process>
Then, the extended Kalman filter 6 receives the pseudorange residual calculated by the pseudorange residual calculation unit 22 and the white line azimuth residual calculated by the white line azimuth residual calculation unit 24 as observation amounts. The navigation error included in the positioning result, the error of the gyro 4 used for the DR calculation, the error of the odometer 5 used for the DR calculation, the GPS clock error of the GPS receiver 1, and the alignment error of the camera 7 are calculated as state quantities. Output as correction amount.

The DR calculation unit 3 corrects the position and azimuth of the vehicle measured based on the navigation error output from the extended Kalman filter 6 and outputs the corrected position and azimuth of the vehicle.
Further, the image processing unit 8 calculates the installation angle of the camera 7 with respect to the vehicle based on the camera alignment error output by the extended Kalman filter 6, and next time, calculates the azimuth angle of the white line reflected in the image based on the calculated installation angle of the camera 7. calculate.

The Kalman filter is an algorithm proposed by Kalman in 1960. It gives the dynamics (time evolution, time change) of the physical quantity (called state quantity) calculated by a linear model, and the probability distribution function of the state quantity error is a Gaussian distribution. In this case, a solution that minimizes the estimation error is given. Here, minimizing the estimation error means that the residual sum of squares is minimized by taking into account the dynamics of the variables selected for the state variables (for example, navigation error, camera alignment error, self-position / attitude angle). Is estimated.
In the case of a discrete time system, a basic equation of a general Kalman filter is expressed by the following state equation (Equation 1) and observation equation (Equation 2).

In particular, the extended Kalman filter 6 (Extended Kalman Filter, EKF) is a Kalman filter that creates an equation linearized around the current estimator when the system equation is nonlinear and constructs the above basic equation based on the equation. is there.
That is, when x and z are expressed as a function f of x _k−1 , u _k , and w _k and a function h of x _k , v _k , respectively, as shown in the following expressions 3 and 4, the Jacobian is used. Then, linearization is performed as shown in Equation 5 and Equation 6.

  The equation of state (Equation 7) and the observation equation (Equation 8) are expressed as follows using F and H obtained by linearization shown in Equations 5 and 6.

  In addition, the Kalman filter operation includes time propagation and observation update.

First, the time propagation of the error covariance matrix P of the state quantity x is expressed by the following equations 9 and 10 using the matrix F representing the dynamics of the system.
For Equation 9, for example, approximate calculation is performed using the second term or up to the third term.

  In addition, the observation update is calculated by the following equations 11 to 13.

In each of the above equations, in the first embodiment, the navigation error included in the self-position and azimuth angle measured by the DR calculation unit 3 and the error of the sensor (gyro 4, odometer 5, GPS clock, etc.) used for the navigation calculation The alignment error of the camera 7 corresponds to the state quantity x, and the pseudorange residual and the white line azimuth angle residual correspond to the observation residual Δz.
The extended Kalman filter 6 estimates the navigation error and the camera alignment error based on the above equations and theories.

In the first embodiment, the following self-positioning apparatus 100 has been described.
The self-positioning device 100 according to the first embodiment includes a monocular camera 7 as hardware, in addition to the existing hardware configuration at the car navigation level (GPS receiver 1 + 1 axis gyro 4 + vehicle speed pulse [odometer 5]).
In addition, the self-localization device 100 according to the first embodiment includes an image processing unit 8 that obtains the azimuth angle of the white line from the image of the camera 7, a road linear database (white line information database 9), and a straight line portion of the white line from the data. An azimuth angle calculation unit (white line azimuth angle acquisition unit 23) that calculates an azimuth angle, a DR operation unit 3 for calculating a self-position / attitude angle, and an extended Kalman filter 6 are provided.
The road alignment database does not need to be constructed from scratch, but it is only necessary to add a new azimuth to the data indicating the straight line portion of the white line, compared to the database constructed according to Patent Document 6 which has already entered the practical use stage. It is a feature. Further, the camera 7 used by the self-positioning apparatus 100 is also characterized in that it can be monocular and can be used at a low price.
The DR calculation unit 3 performs dead reckoning calculation using an inertial device (one-axis gyro 4 (azimuth angle direction)) and vehicle speed pulse data. Then, the extended Kalman filter 6 performs self-position / attitude angle error estimation using the GPS observation data and the white line azimuth angle data as observation amounts, and improves the self-positioning accuracy of the DR operation unit 3.
In other words, the method of using the white line azimuth described in the first embodiment for self-position correction has an advantage that only one monocular camera 7 needs to be added as a hardware configuration. That is, the self-positioning apparatus 100 according to Embodiment 1 does not require a stereo camera or a laser radar for observing the distance from the self-position to the white line. In addition, the angle of the straight line portion of the white line (the azimuth angle of the white line) can be detected relatively accurately from the image captured by the monocular camera 7, and the azimuth angle once measured and recorded in the road linear database is deteriorated due to the earth's crustal movement. It is one of the advantages that the white line azimuth is used for self-position correction. In other words, it is difficult to accurately create a database of road coordinates, and taking into account the coordinate accuracy of digital map data for car navigation currently used, the coordinates or distances calculated based on the coordinates can be used for self-position correction. It is difficult to achieve decimeter-level coordinate accuracy with the method used.
Considering such circumstances, it can be said that the method in the first embodiment using the azimuth angle data of the white line is a promising self-positioning method.

In addition, the self-positioning method according to the first embodiment uses a feature amount having a small change rate of the image edge position in the vehicle movement direction as a straight line portion of a white line as a landmark, and therefore, a low price monocular camera + low price The feature is that the image processor can provide sufficient accuracy.
In the first embodiment, the mounting angle error (alignment error) of the in-vehicle camera 7 can be estimated by the extended Kalman filter 6 and the error can be corrected. For this reason, even when a stereo camera with strict alignment is used, the self-positioning accuracy can be increased.

  By adding the monocular camera 7 to the existing car navigation level hardware configuration and applying the road alignment database and GPS / DR / image composite algorithm by the self-localization method shown in the first embodiment, It is possible to improve the self-localization accuracy in such as.

Embodiment 2. FIG.
In the second embodiment, as a specific example of the self-positioning device 100 described with reference to FIG. 1 in the first embodiment, the azimuth residual calculation unit 130 is viewed from the vehicle instead of the white line azimuth. A mode for calculating a residual (hereinafter referred to as a sign azimuth residual) in a direction in which the road sign is located (hereinafter referred to as a sign azimuth) will be described.
In the second embodiment, the azimuth residual calculation unit 130 calculates a residual (hereinafter referred to as a sign distance residual) for a distance from the vehicle to the road sign (hereinafter referred to as a sign distance). Then, the error estimating unit 140 calculates the navigation error and the camera alignment error by inputting the pseudorange residual, the sign azimuth residual, and the sign distance residual as observation quantities.
Hereinafter, matters different from those of the first embodiment will be described, and items omitted from the description will be the same as those of the first embodiment.

FIG. 6 is a configuration diagram of the self-positioning apparatus 100 according to the second embodiment.
In FIG. 6, the self-localization device 100 according to the second embodiment is different from the self-localization device 100 according to the first embodiment in the white line information database 9, the white line azimuth angle obtaining unit 23, and the white line azimuth angle residual calculation unit. 24, a sign information database 11, a sign azimuth / distance calculator 31, and a sign azimuth / distance residual calculator 32 are provided.
In the self-localization device 100 according to the second embodiment, the image processing unit 8 uses the direction of the road sign relative to the vehicle for the road sign reflected in the image captured by the camera 7 instead of the azimuth angle of the white line. An azimuth angle of the sign indicating, and a sign distance indicating a distance from the vehicle to the road sign are calculated.

  In the sign information database 11 (an example of a position database), position information of each road sign is stored in advance. For example, a road information database in which position information of various landmarks including road signs is registered and generally used in a car navigation system can be used as the sign information database 11.

  The sign azimuth / distance calculation unit 31 (an example of the database distance unit) is based on the position and azimuth of the vehicle calculated by the DR calculation unit 3 and the position of the road sign located in the traveling direction of the vehicle from the sign information database 11. Information is acquired, and the azimuth and distance of the road sign located in the traveling direction of the vehicle are calculated using the CPU.

  The sign azimuth / distance residual calculation unit 32 (an example of the distance residual calculation unit) is a sign azimuth and a sign azimuth / distance calculation unit based on the image calculated by the image processing unit 8 (an example of the image distance unit). The difference with the azimuth angle of the sign based on the sign information database 11 calculated by 31 is calculated as the sign azimuth residual using the CPU. Also, the sign azimuth / distance residual calculation unit 32 signs the difference between the sign distance based on the image calculated by the image processing unit 8 and the sign distance based on the sign information database 11 calculated by the sign azimuth / distance calculation unit 31. The distance residual is calculated using the CPU.

FIG. 7 is a flowchart showing the self-positioning method 2 of the self-positioning device 100 according to the second embodiment.
7, S310 corresponds to the positioning process (S110) of FIG. 2, S321 to S324 correspond to the GPS residual calculation process (S120) of FIG. 2, and S331 to S334b are the azimuth residual calculation process of FIG. Corresponding to (S130), S340 corresponds to the error estimation processing (S140) of FIG.
Each unit of the self-positioning apparatus 100 executes each process described below using a CPU.

<S310 to S324>
S310 to S324 related to the positioning process (S110) and the GPS residual calculation process (S120) are the same as S210 to S224 of the self-positioning method 1 (see FIG. 5) in the first embodiment.

<S331: Example of Imaging Process>
When the DR calculation unit 3 performs positioning, the camera 7 images the traveling direction of the road on which the vehicle is traveling.

<S332a: Example of Image Azimuth Calculation Processing>
Next, the image processing unit 8 detects a road sign reflected in the image from the image captured by the camera 7. For example, the image processing unit 8 detects a road sign that displays a mark of a specific shape based on the shape formed by the feature points extracted by binarizing the image. Further, for example, the image processing unit 8 performs a character recognition process on the image, and detects a road sign displaying a specific character string based on the recognized character information.
Then, the image processing unit 8 calculates a relative angle indicating the direction in which the road sign is viewed from the vehicle based on the position of the road sign on the image (pixel position of the road sign), and the vehicle calculated by the DR calculation unit 3 The relative angle of the road sign on the image is added to the azimuth angle to calculate the azimuth angle of the sign indicating the direction in which the road sign is located from the vehicle. For example, the image processing unit 8 calculates the actual relative angle of the road sign shown in the image with respect to the vehicle by multiplying the number of pixels in the horizontal direction from the center of the image to the pixel position where the road sign is located by an angle formed by one pixel. To do. The angle corresponding to each pixel depends on a camera parameter (for example, focal length) and is calculated based on the camera parameter. For the sign projected as an image on the two-dimensional imaging surface according to the characteristics of the camera 7, the image processing unit 8 determines the position of the sign on the image in the three-dimensional position in the real world according to the characteristics of the camera 7. The actual azimuth angle and distance of the sign are calculated by converting to.
<S332b: Example of Image Distance Calculation Processing>
Further, the image processing unit 8 calculates a sign distance from the vehicle to the road sign based on the position of the road sign on the image.
For example, the image processing unit 8 performs motion stereo vision processing using two images taken by the camera 7 at different successive times, and calculates the sign distance from the vehicle to the road sign.

<S333a>
Next, the sign azimuth / distance calculation unit 31 acquires the position information of the road sign located in the traveling direction of the vehicle from the sign information database 11 based on the position and azimuth of the vehicle measured by the DR calculation unit 3. . That is, the sign azimuth / distance calculation unit 31 acquires position information from the sign information database 11 for the road sign reflected in the image captured by the camera 7.
<S333b>
Next, the sign azimuth / distance calculation unit 31 is based on the position of the road sign acquired from the sign information database 11 and the position of the vehicle calculated by the DR calculation unit 3. And the relative angle of the road sign seen from the vehicle is added to the azimuth of the vehicle calculated by the DR calculation unit 3 to calculate the azimuth of the road sign with the vehicle as the base point.
<S333c>
Further, the sign azimuth / distance calculation unit 31 calculates the sign distance from the vehicle to the road sign based on the position of the road sign acquired from the sign information database 11 and the position of the vehicle calculated by the DR calculation unit 3. .

<S334a>
Next, the sign azimuth / distance residual calculation unit 32 calculates the sign azimuth calculated by the image processing unit 8 based on the image and the sign azimuth / distance calculation unit 31 calculated based on the sign information database 11. The difference from the azimuth is calculated as the sign azimuth residual.
<S334b>
Also, the sign azimuth / distance residual calculation unit 32 calculates the difference between the sign distance calculated by the image processing unit 8 based on the image and the sign distance calculated by the sign azimuth / distance calculation unit 31 based on the sign information database 11. Is calculated as the marker distance residual.

<S340>
The extended Kalman filter 6 inputs the pseudorange residual calculated by the pseudorange residual calculation unit 22, the sign azimuth residual and the sign distance residual calculated by the sign azimuth / distance residual calculation unit 32 as observation quantities. Then, the navigation error included in the positioning result of the DR calculation unit 3 and the alignment error of the camera 7 are calculated as state quantities and output as correction amounts.

In the second embodiment, the following self-positioning device 100 has been described.
The self-positioning device 100 according to the second embodiment includes a monocular camera 7 as hardware, in addition to the existing hardware configuration of a car navigation level (GPS receiver 1 + 1 axis gyro 4 + vehicle speed pulse [odometer 5]).
In addition, the self-positioning device 100 according to the second embodiment includes an image processing unit 8 that obtains “the distance between the vehicle and the sign” and “the relative azimuth angle of the sign as seen from the vehicle” from the image of the camera 7, and the sign coordinates. A database (sign information database 11), a DR operation unit 3 for calculating a self-position / attitude angle, and an extended Kalman filter 6 are provided.
The DR calculation unit 3 performs dead reckoning calculation using an inertial device (one-axis gyro 4 (azimuth angle direction)) and vehicle speed pulse data. Then, the extended Kalman filter 6 estimates the self-position / attitude angle error using the GPS observation data and the marker data as observation quantities, and improves the self-positioning accuracy of the DR calculation unit 3.

Embodiment 3 FIG.
In the third embodiment, as a specific example of the self-positioning apparatus 100 described with reference to FIG. 1 in the first embodiment, the azimuth residual calculation unit 130 performs the white line azimuth residual described in the first embodiment. A mode for calculating the sign azimuth residual and the sign distance residual described in the second embodiment will be described.
In the third embodiment, the positioning unit 110 calculates the self position by correcting the self position calculated by dead reckoning by map matching using data in the road database 49.
In the third embodiment, the error estimation unit 140 inputs the pseudorange residual, the white line azimuth residual, the sign azimuth residual, and the sign distance residual as observation amounts, and calculates a navigation error and a camera alignment error. calculate.
Hereinafter, matters different from those in the first embodiment and the second embodiment will be described, and items that are not described here are the same as those in the first embodiment or the second embodiment.

FIG. 8 is a configuration diagram of the self-positioning apparatus 100 according to the third embodiment.
In FIG. 8, the self-positioning device 100 according to the third embodiment is different from the self-positioning device 100 according to the first embodiment in that the white line information database 9, the white line azimuth angle obtaining unit 23, and the white line azimuth angle residual calculating unit. 24, a road database 49, an azimuth / distance calculation unit 41, and an azimuth / distance residual calculation unit 42 are provided.
Further, the self-location locating apparatus 100 according to Embodiment 3 is characterized in that the DR calculation unit 3 acquires road information around the vehicle from the road database 49 and corrects the position of the vehicle by map matching.

  The road database 49 includes various information such as white line information in the white line information database 9 described in the first embodiment, road sign information described in the second embodiment, and road lane information (for example, position, width, up and down). Road information is stored in advance.

  The azimuth / distance calculation unit 41 is based on the position and azimuth of the vehicle calculated by the DR calculation unit 3, and the road located in the azimuth angle of the white line located in the traveling direction of the vehicle and the traveling direction of the vehicle from the road database 49. The sign position information is acquired, and the azimuth angle of the road sign located in the traveling direction of the vehicle and the sign distance from the vehicle to the road sign are calculated using the CPU.

  The azimuth / distance residual calculation unit 42 calculates the difference between the azimuth angle of the white line based on the image calculated by the image processing unit 8 and the azimuth angle of the white line based on the road database 49 acquired by the azimuth / distance calculation unit 41. An azimuth residual is calculated using a CPU. Further, the azimuth / distance residual calculation unit 42 is a difference between the azimuth angle of the sign based on the image calculated by the image processing unit 8 and the azimuth angle of the sign based on the road database 49 calculated by the azimuth / distance calculation unit 41. Is calculated using the CPU as the sign azimuth residual. The azimuth / distance residual calculation unit 42 calculates the difference between the sign distance based on the image calculated by the image processing unit 8 and the sign distance based on the road database 49 calculated by the azimuth / distance calculation unit 41. It calculates using CPU.

FIG. 9 is a flowchart showing the self-positioning method 3 of the self-positioning device 100 according to the third embodiment.
9, S410a to S410c correspond to the positioning process (S110) of FIG. 2, S421 to S424 correspond to the GPS residual calculation process (S120) of FIG. 2, and S431 to S434c represent the azimuth residual of FIG. Corresponding to the calculation process (S130), S440 corresponds to the error estimation process (S140) of FIG.
Each unit of the self-positioning apparatus 100 executes each process described below using a CPU.

<S410a>
First, the DR calculation unit 3 calculates the position and azimuth of the vehicle by dead reckoning.
<S410b>
Next, the DR calculation unit 3 acquires peripheral road information from the road database 49 based on the calculated vehicle position.
<S410c: Example of positioning process>
Next, based on the lane information indicated by the road information acquired from the road database 49, the DR calculation unit 3 sets the vehicle so that the vehicle is positioned on the lane corresponding to the traveling direction (azimuth angle) and the traveling direction of the vehicle. Map matching (or lane keeping) is performed to correct the position. The DR calculation unit 3 calculates the vehicle position and azimuth by correcting the position and azimuth of the vehicle for which map matching has been performed based on the navigation error calculated by the extended Kalman filter 6. The DR calculation unit 3 uses a process generally performed in a car navigation system as a map matching process. However, the map matching process has a self-localization accuracy when the driving lane is clear, such as when there is only one lane on the road, or when the state quantity error standard deviation of the extended Kalman filter exceeds an appropriately set threshold. It shall be performed only when it is estimated that the condition has deteriorated.

<S421 to S424>
S421 to S424 related to the GPS residual calculation process (S120) are the same as S221 to S224 of the self-positioning method 1 (see FIG. 5) in the first embodiment.

<S431 to S434c>
S431, S432a, S433a, and S434a related to the white line azimuth residual calculation processing are the same as S231 to S234 of the self-positioning method 1 (see FIG. 5) in the first embodiment.
Further, S432b, S433b and S434b relating to the azimuth residual calculation process and the sign distance residual calculation process of the road sign are the same as S332a to S334b of the self-localization method 2 (see FIG. 7) in the second embodiment. .

  The extended Kalman filter 6 calculates the pseudorange residual calculated by the pseudorange residual calculation unit 22, the white line azimuth residual, the sign azimuth residual, and the sign distance residual calculated by the azimuth / distance residual calculation unit 42. Is input as an observation amount, and the navigation error and the alignment error of the camera 7 included in the positioning result by dead reckoning of the DR operation unit 3 are calculated as state amounts and output as correction amounts.

  In the above description, only the azimuth angle is targeted for the white line, but the residual of the distance from the vehicle to the white line may be used as the observation amount of the extended Kalman filter 6 as in the case of the sign, and the length of the white line calculated based on the image and the database The residual with the length of the white line acquired from the above may be used as the observation amount of the extended Kalman filter 6.

In the third embodiment, the following self-positioning apparatus 100 has been described.
The self-positioning device 100 according to the third embodiment is different from the self-positioning device 100 described in the first embodiment in the “distance between the vehicle and the sign” described in the second embodiment and the sign of the sign viewed from the vehicle. The function of the image processing unit 8 for obtaining the “relative azimuth” and the database of the marker coordinates are added, and the extended Kalman filter 6 performs error estimation of the self-position / attitude angle using both the white line and the marker as observation amounts, and the DR calculation unit The self-localization accuracy of 3 is improved.

Embodiment 4 FIG.
In the first embodiment, the form using the white line information database 9 in which information such as position, shape, and azimuth angle is stored in advance for each white line has been described. However, it may be difficult to prepare information for each white line in terms of cost.
Therefore, in the fourth embodiment, instead of the white line information database 9 shown in the first embodiment, a database that stores road information (road alignment information) indicating the azimuth angle of the road is used, and the azimuth angle of the road is set to the white line information. The form used as the azimuth will be described.

(1) Method using KIWI database As a database for storing road information, a database used in a car navigation system can be used. The car navigation system database uses the KIWI format. Hereinafter, a database indicating road information in the KIWI format (Japanese Industrial Standard JIS-D0810) is referred to as a KIWI database. Non-Patent Document 1 shows documents related to the KIWI format.
The data structure of the KIWI database includes node information indicating information about intersections, link information indicating intersections to which roads connect, and an extension unit that can newly add information. In the node information, node names and intersection coordinates (for example, two-dimensional coordinates at the center of the intersection) are stored in association with each intersection as a node. The link information stores information indicating an intersection where roads are connected, such as “node A (intersection A) and node B (intersection B) are connected (A, B)”. In the fourth embodiment, the link information (connection information) and the azimuth are associated with the extension unit of the KIWI database, and the azimuth information indicating the azimuth is stored for the road represented by the link information. For example, when the node information and the link information show the relationship as shown in FIG. 11, (A, B) is stored as “connection information”, θ1 is stored as “azimuth angle”, and “connection information”. (B, C), and θ2 is stored as the “azimuth angle”.

  Then, as shown in FIG. 12, the white line azimuth obtaining unit 23 in the first embodiment obtains road information from the KIWI database, and calculates the road azimuth as the white line azimuth.

<S510>
The white line azimuth angle obtaining unit 23 obtains the self position N and the self azimuth angle φN from the DR calculation unit 3.
<S520>
Next, the white line azimuth obtaining unit 23 searches the link information indicating the road on which the vehicle is located by comparing the coordinates of each intersection indicated by the node information with the self position N in the KIWI database.
<S530>
Next, the white line azimuth angle obtaining unit 23 extracts the road azimuth angle θn corresponding to the searched link information from the expansion unit.
<S540>
Next, the white line azimuth angle acquisition unit 23 acquires the azimuth angle θn of the road in the traveling direction indicated by the self azimuth angle φN. However, since the expression of the azimuth angle differs by 180 degrees depending on the direction of viewing the white line, 180 degrees may be added to θn depending on the self-azimuth angle φN.
<S550>
Then, the white line azimuth angle obtaining unit 23 outputs the acquired road azimuth angle θn as a white line azimuth angle.

(2) Method Using Road Alignment Database Although the geometric structure of a road can be expressed by a cross section and a plane alignment, the road alignment database used in Embodiment 4 is a straight line, a circular curve, a relaxation curve ( Road information expressed by linear elements such as clothoid) is stored. The straight line of the road is expressed by, for example, the coordinates of the start point and the coordinates of the end point.
Then, the white line azimuth acquisition unit 23 according to the fourth embodiment calculates the road azimuth as the white line azimuth based on the straight line data of the road stored in the road alignment database as shown in FIG. To do.

<S610>
The white line azimuth acquisition unit 23 acquires the self position N from the DR calculation unit 3.
<S620>
Next, the white line azimuth obtaining unit 23 searches the road linear database for data on the straight line portion of the road where the self position N is located.
<S630>
Next, the white line azimuth obtaining unit 23 calculates the azimuth angle of the road based on the coordinates of the start point and the end point indicated by the data of the searched straight portion of the road. However, the azimuth angle of the road may be stored in advance in the road alignment database.
<S640>
Then, the white line azimuth obtaining unit 23 outputs the calculated road azimuth as the white line azimuth.

In each embodiment, the GPS receiver 1 corrects pseudo distances using GPS correction information from MSSAT (MTSAT Satellite-based Augmentation System) or GPS correction information obtained by receiving FM multiplex broadcasting. Is also possible. In the future, the GPS correction information is scheduled to be included in the broadcast data from the quasi-zenith satellite, and a method in which the GPS receiver 1 uses this to perform GPS pseudo-range correction is also conceivable.
In addition, a method in which the GPS receiver 1 smooths the pseudorange using carrier phase observation data (carrier smoothing), a carrier wave phase data of the GPS reference station and a GPS carrier phase data that the DR receiver 3 observes with the GPS receiver 1 A method of performing positioning by performing CPDGPS (Carrier Phase Differential GPS) calculation using the GPS is also conceivable.
Furthermore, the configuration method of the extended Kalman filter 6 is a tight coupling (tightly coupled) method using the GPS observation raw data (pseudorange, phase difference, etc.) described in the first embodiment as a GPS observation amount. In addition, a loose coupling (coarse coupling) method that uses a positioning solution obtained from a GPS observation value as a GPS observation amount is also conceivable.

1 is a configuration diagram of a self-positioning device 100 according to Embodiment 1. FIG. 3 is a flowchart showing a self-positioning method of self-positioning device 100 in the first embodiment. FIG. 3 is a diagram illustrating an example of hardware resources of the self-location apparatus 100 according to the first embodiment. 1 is a configuration diagram of a self-positioning device 100 according to Embodiment 1. FIG. 3 is a flowchart showing a self-positioning method GPS receiver 1 of the self-positioning device 100 according to the first embodiment. The block diagram of the self-positioning apparatus 100 in Embodiment 2. FIG. 10 is a flowchart showing a self-positioning method 2 of the self-positioning device 100 according to the second embodiment. The block diagram of the self-positioning apparatus 100 in Embodiment 3. FIG. 10 is a flowchart showing a self-positioning method of self-positioning apparatus 100 according to Embodiment 3. FIG. 3 is a relationship diagram between the azimuth angle of a white line and a white line (a straight line portion thereof) and the lane direction in the first embodiment. FIG. 10 is a relationship diagram of KIWI data in the fourth embodiment. 10 is a flowchart of azimuth angle acquisition processing in the fourth embodiment. 10 is a flowchart of azimuth angle acquisition processing in the fourth embodiment.

Explanation of symbols

  1 GPS receiver, 2 satellite orbit calculation unit, 3 DR calculation unit, 4 gyro, 5 odometer, 6 extended Kalman filter, 7 camera, 8 image processing unit, 9 white line information database, 11 sign information database, 21 satellite distance calculation unit, 22 pseudorange residual calculation unit, 23 white line azimuth acquisition unit, 24 white line azimuth residual calculation unit, 31 sign azimuth / distance calculation unit, 32 sign azimuth / distance residual calculation unit, 41 azimuth / distance calculation , 42 azimuth / distance residual calculation unit, 49 road database, 100 self-localization device, 110 positioning unit, 120 GPS observation residual calculation unit, 121 GPS observation unit, 122 GPS observation amount calculation unit, 130 azimuth residual Difference calculation unit, 131 Azimuth angle observation unit, 132 Azimuth angle calculation unit, 140 Error estimation unit, 901 Display device, 911 CPU, 91 Bus, 913 ROM, 914 RAM, 915 communication board, 920 a magnetic disk device, 921 OS, 923 programs, 924 files.

Claims (9)

A positioning unit for positioning the self-position;
A camera that captures a specific direction when the positioning unit performs positioning;
Based on the image captured by the camera, the azimuth angle of the specific feature formed by the length direction of the specific feature shown in the image with respect to the reference direction is calculated using a CPU (Central Processing Unit). Image azimuth,
An azimuth angle database that stores the azimuth angle of the feature that the length direction of the feature forms with respect to the reference direction;
A database azimuth section for obtaining an azimuth angle of the feature corresponding to the specific feature from the azimuth database based on the self-position measured by the positioning section;
An azimuth residual calculation unit that calculates the difference between the azimuth of the feature calculated by the image azimuth and the azimuth of the feature acquired by the database azimuth using the CPU as a feature azimuth residual. When,
A Kalman filter that uses a CPU to estimate the error of the self-position measured by the positioning unit based on the feature azimuth residual calculated by the azimuth residual calculating unit;
The positioning unit corrects the measured self-position based on the error estimated by the Kalman filter, and uses the corrected self-position as a positioning result. A positioning unit for positioning the self-position;
A camera that captures a specific direction when the positioning unit performs positioning;
An image azimuth section that calculates a azimuth angle of a specific feature reflected in the image based on an image captured by the camera using a CPU (Central Processing Unit) based on the self-position;
A location database that stores the location of features;
The positioning unit acquires the position of the feature corresponding to the specific feature from the position database based on the self-position measured by the positioning unit, and the positioning unit calculates the azimuth angle of the feature based on the acquired feature position. A database azimuth angle portion calculated using a CPU with the measured self-position as a base point;
An azimuth residual calculation unit that calculates the difference between the azimuth of the feature calculated by the image azimuth and the azimuth of the feature calculated by the database azimuth using the CPU as a feature azimuth residual. When,
A Kalman filter that uses a CPU to estimate the error of the self-position measured by the positioning unit based on the feature azimuth residual calculated by the azimuth residual calculating unit;
The positioning unit corrects the measured self-position based on the error estimated by the Kalman filter, and uses the corrected self-position as a positioning result. The Kalman filter is
Relationship between the observed quantity and the state quantity for the state equation that modeled the dynamics of the state quantity for the state quantity that includes the self-position error, and the observed quantity that contains the feature azimuth residual and the state quantity that contained the self-position error 3. The self-positioning apparatus according to claim 1, wherein error estimation of a state quantity is performed based on an observation equation formulated. The self-positioning device further includes:
An image distance unit for calculating a distance to the specific feature using a CPU based on an image captured by the camera;
A location database that stores the location of features;
The position of the feature corresponding to the specific feature is acquired from the position database based on the self-position measured by the positioning unit, and the position of the feature acquired from the self-position measured by the positioning unit and the position database. A database distance part that calculates the distance to the feature based on
A distance residual calculation unit that calculates the difference between the distance to the feature calculated by the image distance unit and the distance to the feature calculated by the database distance unit using the CPU as a feature distance residual;
4. The self-localization device according to claim 3, wherein the Kalman filter includes a feature distance residual calculated by the distance residual calculation unit in an observation amount and performs error estimation of a state quantity including a self-position error. . The positioning unit performs a positioning process to measure its own position,
The camera performs an imaging process for imaging a specific direction when the positioning unit performs positioning,
The azimuth angle of the specific feature that the length direction of the specific feature shown in the image is based on the image captured by the camera with respect to the reference direction is the CPU (Central Processing Unit). Perform the image azimuth calculation process to calculate using
The database azimuth portion corresponds to the specific feature from the azimuth angle database that stores the azimuth angle of the feature that the length direction of the feature forms with respect to the reference direction based on the self-position measured by the positioning unit. Perform database azimuth acquisition processing to acquire the azimuth of features,
The azimuth residual calculation unit calculates the difference between the azimuth of the feature calculated by the image azimuth and the azimuth of the feature acquired by the database azimuth using the CPU as the feature azimuth residual. Azimuth residual calculation process to
An error estimation process is performed in which the Kalman filter estimates an error of the self-position measured by the positioning unit based on the feature azimuth residual calculated by the azimuth residual calculating unit using a CPU,
The positioning unit corrects a measured self-position based on an error estimated by the Kalman filter, and performs a positioning result correction process using the corrected self-position as a positioning result. The positioning unit performs a positioning process to measure its own position,
The camera performs an imaging process for imaging a specific direction when the positioning unit performs positioning,
An image azimuth calculation process in which an image azimuth is calculated by using a CPU (Central Processing Unit) based on an image captured by the camera, based on an azimuth angle of a specific feature reflected in the image. Done
The database azimuth section acquires the position of the feature corresponding to the specific feature from the position database that stores the position of the feature based on the self-position measured by the positioning unit, and based on the acquired position of the feature Database azimuth calculation processing is performed to calculate the azimuth of the feature using the CPU with the self-position determined by the positioning unit as a base point,
The azimuth residual calculation unit calculates the difference between the azimuth of the feature calculated by the image azimuth and the azimuth of the feature calculated by the database azimuth using the CPU as the feature azimuth residual. Azimuth residual calculation process to
An error estimation process is performed in which the Kalman filter estimates an error of the self-position measured by the positioning unit based on the feature azimuth residual calculated by the azimuth residual calculation unit using a CPU,
The positioning unit corrects a measured self-position based on an error estimated by the Kalman filter, and performs a positioning result correction process using the corrected self-position as a positioning result.   A self-positioning program that causes a computer to execute the self-positioning method according to claim 5. The image azimuth portion calculates the azimuth angle of the white line reflected in the image,
The azimuth angle database stores road information indicating the azimuth angle of the road that the straight line portion of the road forms with respect to the reference direction in a KIWI format;
The database azimuth section obtains the azimuth angle of the straight line portion of the road stored in the azimuth angle database as the azimuth angle of the white line marked on the road,
The azimuth residual calculation unit calculates an azimuth residual of a white line as the feature azimuth residual,
The self-position locating apparatus according to claim 1, wherein the Kalman filter estimates an error of a self-position based on an azimuth residual of a white line. The image azimuth portion calculates the azimuth angle of the white line reflected in the image,
The azimuth database stores road information indicating a planar shape of a road by at least one of an arc, a straight line, and a clothoid;
The database azimuth section calculates the azimuth of the straight portion of the road as the azimuth of the white line marked on the road based on the planar shape of the road indicated by the road information acquired from the azimuth database,
The azimuth residual calculation unit calculates an azimuth residual of a white line as the feature azimuth residual,
The self-position locating apparatus according to claim 1, wherein the Kalman filter estimates an error of a self-position based on an azimuth residual of a white line.

Images