WO2023176617A1 - 位置推定装置、位置推定方法、およびプログラム - Google Patents

位置推定装置、位置推定方法、およびプログラム Download PDF

Info

Publication number
WO2023176617A1
WO2023176617A1 PCT/JP2023/008792 JP2023008792W WO2023176617A1 WO 2023176617 A1 WO2023176617 A1 WO 2023176617A1 JP 2023008792 W JP2023008792 W JP 2023008792W WO 2023176617 A1 WO2023176617 A1 WO 2023176617A1
Authority
WO
WIPO (PCT)
Prior art keywords
unit
superimposed
data
point
estimation device
Prior art date
Application number
PCT/JP2023/008792
Other languages
English (en)
French (fr)
Inventor
正浩 加藤
Original Assignee
パイオニア株式会社
パイオニアスマートセンシングイノベーションズ株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by パイオニア株式会社, パイオニアスマートセンシングイノベーションズ株式会社 filed Critical パイオニア株式会社
Publication of WO2023176617A1 publication Critical patent/WO2023176617A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging

Definitions

  • the present invention relates to a position estimation device, a position estimation method, and a program.
  • Patent Documents 1 and 2 disclose methods of estimating the vehicle position using the results of detecting marking lines such as white lines provided on roads.
  • An example of the problem to be solved by the present invention is to provide a technique that can estimate the position of an object with high accuracy even if the data points are sparse.
  • the invention according to claim 1 includes: a first acquisition unit that acquires point cloud data at multiple timings obtained by a sensor mounted on the moving body; a second acquisition unit that acquires movement information of the mobile object; a superimposition unit that generates superimposed point cloud data in which the point cloud data at the plurality of timings are superimposed based on the movement information; a first estimation unit that estimates the position of the object using the superimposed point group data; It is a position estimation device.
  • the invention according to claim 12 is A computer-implemented position estimation method, comprising: a first acquisition step of acquiring point cloud data at multiple timings obtained by a sensor mounted on the moving body; a second acquisition step of acquiring movement information of the mobile object; a superimposing step of generating superimposed point cloud data in which the point cloud data at the plurality of timings are superimposed based on the movement information; a first estimation step of estimating the position of the object using the superimposed point group data; This is a position estimation method.
  • the invention according to claim 13 is A program that causes a computer to execute the position estimation method according to claim 12.
  • FIG. 1 is a diagram illustrating a configuration of a position estimation device according to an embodiment.
  • FIG. 2 is a diagram illustrating how a moving object detects a white line on a road.
  • (a) and (b) are diagrams showing data points in the first region when the sensor resolution is high and low, respectively.
  • 2 is a flowchart illustrating the flow of processing performed by the position estimation device according to the embodiment.
  • FIG. 1 is a block diagram illustrating the configuration of a position estimation device according to an example. It is a flowchart which illustrates the flow of self-position estimation processing performed by the position estimation device concerning an example.
  • FIG. 2 is a diagram illustrating a computer for realizing a position estimation device.
  • FIG. 7 is a diagram showing the relationship between reflection intensity and distance before and after correction.
  • FIG. 3 is a diagram for explaining superimposition of point cloud data.
  • FIG. 3 is a diagram for explaining superimposition of point cloud data.
  • FIG. 6 is a diagram for explaining a method in which a coordinate transformation unit performs coordinate transformation of point group data using velocity v and yaw angular velocity ⁇ '.
  • FIG. 6 is a diagram for explaining a method in which a coordinate transformation unit performs coordinate transformation of point group data using velocity v, yaw angular velocity ⁇ ', roll angular velocity ⁇ ', and pitch angular velocity ⁇ '.
  • FIG. 3 is a diagram for explaining superimposition frequency control.
  • FIG. 3 is a diagram for explaining extraction of high reflection intensity points.
  • FIG. 7 is a diagram for explaining the meaning of a second area.
  • FIG. 3 is a diagram for explaining principal component analysis of a point cloud.
  • FIG. 3 is a diagram for explaining detection values and reliability information of white lines.
  • FIG. 3 is a diagram for explaining detection values and reliability information of white lines. It is a figure which illustrates the positional relationship between a moving body and a white line.
  • 7 is a flowchart illustrating the flow of processing in S70 shown in FIG. 6.
  • FIG. 1 is a diagram illustrating the configuration of a position estimating device 10 according to an embodiment.
  • the position estimation device 10 includes a first acquisition section 120, a second acquisition section 140, a superimposition section 160, and a first estimation section 180.
  • the first acquisition unit 120 acquires point cloud data at multiple timings obtained by a sensor mounted on a moving object.
  • the second acquisition unit 140 acquires movement information of a mobile object.
  • the superimposition unit 160 generates superimposed point group data in which point group data at a plurality of timings are superimposed based on movement information.
  • the first estimation unit 180 estimates the position of the object using the superimposed point group data. Note that the point cloud data at multiple timings is, for example, point cloud data at multiple times.
  • a moving object such as a vehicle
  • the position of the moving object at the next point in time is estimated using the position of the moving object at a certain point in time and the moving speed and direction of movement of the moving object up to the next point in time.
  • detecting an object (object) around the moving body using a sensor or the like and comparing it with the position of the object in the map information, it is possible to perform correction to further improve the estimation accuracy of the position of the moving body.
  • the object whose position is estimated is a line on the road surface.
  • the target object is a dividing line on the road surface, which is a broken line.
  • the color of the line that is the object is not particularly limited, but may be yellow or white, for example.
  • the surface of the object is formed of a retroreflective material.
  • FIG. 2 is a diagram illustrating how the moving object 20 detects the white line 30 on the road.
  • the sensor mounted on the moving body 20 measures an area inside the measurement area 40.
  • the position estimating device 10 extracts measurement data within the first area 41 and uses it for detecting the white line 30, as will be described in detail later.
  • the senor measures the distance to the target object by emitting light and receiving the reflected light reflected by the target object.
  • the sensor is not particularly limited, but may be a radar or lidar (LIDAR: Laser Imaging Detection and Ranging, Laser Illuminated Detection and Ranging, or LiDAR: Light Detection and Ranging).
  • the light emitted from the sensor is not particularly limited, it is, for example, infrared light.
  • the light emitted from the sensor is, for example, a laser pulse.
  • the sensor calculates the distance from the sensor to the object using, for example, the time from emitting pulsed light to receiving the reflected light and the propagation speed of the pulsed light.
  • the direction in which light is emitted from the sensor is variable, and the measurement area 40 is scanned by sequentially performing measurements in a plurality of directions.
  • the sensor outputs point cloud data in which the three-dimensional position of the light reflection point and the reflection intensity (that is, the light reception intensity at the sensor) are associated.
  • the first acquisition unit 120 of the position estimation device 10 acquires the point cloud data.
  • the point cloud data output from the sensor is composed of frames.
  • One frame is composed of data obtained by scanning the measurement area 40 once.
  • the sensor repeatedly scans the measurement area 40 to generate a plurality of consecutive frames. For example, by moving the emitted light vertically while reciprocating horizontally, the distance to an object in each angular direction can be measured, so three-dimensional information data within the horizontal and vertical scanning range can be measured. Obtainable.
  • FIGS. 3(a) and 3(b) are diagrams showing data points within the first region 41 when the sensor resolution is high and low, respectively.
  • both white circles and black circles are data points, indicating that the light reflection intensity is higher in the black circles than in the white circles.
  • an image of the estimated direction of the white line 30 is shown by a broken arrow.
  • the white line 30 is drawn with a retroreflective material, the reflectance of light is higher than that of the surrounding road surface, and the reflection intensity is higher. Therefore, the first area 41 is provided in the measurement area 40 at a location where the white line 30 can be easily detected, and the white line 30 is recognized by detecting a portion with high reflection intensity within the first area 41 .
  • the position of the end of the white line 30 and the direction of the white line 30 are clear, as shown in FIG. 3(a).
  • the longitudinal direction of the white line 30 is the vertical direction and the width direction is the horizontal direction, a plurality of points on the white line 30 are obtained for each line in the horizontal direction, so the width of the white line 30 can be determined from the interval between points at both ends where the white line 30 is detected.
  • the center line of the white line 30 can be determined by determining whether the line is equal to the line, determining the center point for each line, and connecting them. Further, it can be determined that there is an end point of a broken line between a line where a point on the white line 30 is detected and a line where a point is not detected.
  • the superimposing unit 160 generates superimposed point group data in which point group data at a plurality of timings are superimposed based on movement information.
  • the first estimation unit 180 estimates the position of the object using the superimposed point group data. Therefore, even if the data points obtained in each frame are sparse, it is possible to generate superimposed point group data with dense data points and estimate the position of objects such as lines on the road with high accuracy.
  • FIG. 4 is a flowchart illustrating the flow of processing performed by the position estimation device 10 according to the present embodiment.
  • the position estimation method according to this embodiment is executed by a computer.
  • the position estimation method according to this embodiment includes a first acquisition step S101, a second acquisition step S102, a superimposition step S103, and a first estimation step S104.
  • the first acquisition step S101 point cloud data at multiple timings obtained by a sensor mounted on a moving object is acquired.
  • the second acquisition step S102 movement information of the moving body is acquired.
  • superimposing step S103 superimposed point group data is generated in which point group data at a plurality of timings are superimposed based on movement information.
  • the first estimation step S104 the position of the object is estimated using the superimposed point group data.
  • FIG. 5 is a block diagram illustrating the configuration of the position estimating device 10 according to the embodiment.
  • the position estimating device 10 according to the present example has the configuration of the position estimating device 10 according to the embodiment.
  • the position estimation device 10 according to the present embodiment further includes a first region data extraction section 110, an intensity correction section 150, and a second estimation section 190.
  • the superimposition unit 160 according to this embodiment includes a superimposition point group data generation unit 161, a coordinate transformation unit 163, and a storage unit 165.
  • the first estimation unit 180 according to the present embodiment includes a high intensity point extraction unit 181, a principal component analysis unit 183, a first identification unit 185, a reliability information generation unit 187, and a second identification unit 189. This will be explained in detail below.
  • FIG. 6 is a flowchart illustrating the flow of self-position estimation processing performed by the position estimation device 10 according to the embodiment.
  • the second estimator 190 estimates the position of the moving body 20 using the result of estimating the position of the object by the first estimator 180.
  • the overall flow of the process for estimating the position of the mobile object 20 performed by the position estimating device 10 will be described with reference to this figure.
  • the target object will be described as a white line, but the target object may be other than the white line.
  • the sensor is described as a lidar, other types of sensors may be used.
  • the position estimating device 10 When the operation of the position estimating device 10 is started, the position estimating device 10 first specifies the first estimated self-position (S10). Specifically, the second estimating unit 190 uses the positioning result by a GNSS (Global Navigation Satellite System) provided in the mobile object 20 or the position estimating device 10 as the first estimated self-position. Next, the second estimation unit 190 calculates the latest predicted self-position using the previous estimated self-position (S20). Specifically, the second estimation unit 190 acquires the speed and yaw angular velocity of the mobile body 20 from a speed sensor and a gyro sensor provided in the mobile body 20, and determines the moving direction of the mobile body 20 from the previous estimated self-position. and determine the amount of movement. Then, the second estimating unit 190 calculates the position after movement when the previous estimated self-position is moved in the specified movement direction and movement amount as the predicted self-position.
  • GNSS Global Navigation Satellite System
  • the second estimation unit 190 determines whether a map around the predicted self-location has already been obtained (S30). If the map has already been acquired (Yes in S30), the second estimation unit 190 performs the process in S50 without performing the process in S40. If the map has not been acquired yet (No in S30), the second estimation unit 190 acquires a landmark map around the predicted self-position (S40).
  • the second estimation unit 190 may acquire the landmark map by reading it from a storage unit that is accessible from the second estimation unit 190, or may acquire it from outside through a network.
  • the storage unit that can be accessed from the second estimating unit 190 may be provided inside the position estimating device 10 or may be provided outside the position estimating device 10.
  • the second estimation unit 190 obtains information on a white line located at a distance detectable by a lidar from the moving body 20 from the map, and calculates a measured predicted value of the white line (S50).
  • the predicted measurement value is a predicted value of how the white line will be measured when viewed from the moving body 20.
  • the second estimating unit 190 specifies information on a white line located at a detectable distance from the mobile object 20 in the landmark map. Then, a measured predicted value of the white line is calculated based on the position and direction of the white line shown on the landmark map and the predicted self-position.
  • the first acquisition unit 120 determines whether the point cloud data has been acquired (S60). If point cloud data is not acquired (No in S60), such as when occlusion by another vehicle or the like occurs, the process returns to S20.
  • point cloud data is acquired by the first acquisition unit 120 (Yes in S60)
  • white line detection by point cloud superposition and reliability information generation are performed in S70. S70 will be described in detail later with reference to FIG. 20.
  • the second estimating unit 190 uses these information to correct the predicted self-position calculated in S20 (S80).
  • the method of using the predicted self-position for correction is not particularly limited, for example, extended Kalman filter processing can be used.
  • the second estimation unit 190 sets the corrected predicted self-position as the latest estimated self-position.
  • the estimated self-position may be output to a device other than the position estimating device 10, or may be held in a storage device that is accessible from the second estimation unit 190.
  • the estimated self-position can be used for functions such as route navigation, automatic driving, or driving assistance.
  • the second estimation unit 190 determines whether the termination condition is satisfied (S90).
  • the case where the termination condition is satisfied is, for example, a case where the operation of the moving body 20 is stopped or a case where an operation to stop the estimation process by the position estimating device 10 is performed. If the termination condition is satisfied (Yes in S90), the position estimation device 10 terminates the process. If the termination condition is not met (No in S90), the process returns to S20.
  • Each functional component of the position estimating device 10 may be realized by hardware that implements each functional component (e.g., a hard-wired electronic circuit, etc.), or by a combination of hardware and software (e.g., (e.g., a combination of an electronic circuit and a program that controls it).
  • a combination of hardware and software e.g., (e.g., a combination of an electronic circuit and a program that controls it).
  • FIG. 7 is a diagram illustrating a computer 1000 for realizing the position estimation device 10.
  • Computer 1000 is any computer.
  • the computer 1000 is a SoC (System On Chip), a Personal Computer (PC), a server machine, a tablet terminal, a smartphone, or the like.
  • the computer 1000 may be a dedicated computer designed to implement the position estimation device 10, or may be a general-purpose computer.
  • the computer 1000 has a bus 1020, a processor 1040, a memory 1060, a storage device 1080, an input/output interface 1100, and a network interface 1120.
  • the bus 1020 is a data transmission path through which the processor 1040, memory 1060, storage device 1080, input/output interface 1100, and network interface 1120 exchange data with each other.
  • the processor 1040 is a variety of processors such as a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), or an FPGA (Field-Programmable Gate Array).
  • the memory 1060 is a main storage device implemented using RAM (Random Access Memory) or the like.
  • the storage device 1080 is an auxiliary storage device implemented using a hard disk, an SSD (Solid State Drive), a memory card, a ROM (Read Only Memory), or the like.
  • the input/output interface 1100 is an interface for connecting the computer 1000 and an input/output device.
  • an input device such as a keyboard and an output device such as a display are connected to the input/output interface 1100.
  • the network interface 1120 is an interface for connecting the computer 1000 to a network.
  • This communication network is, for example, a LAN (Local Area Network) or a WAN (Wide Area Network).
  • the method by which the network interface 1120 connects to the network may be a wireless connection or a wired connection.
  • the storage device 1080 stores program modules that implement each functional component of the position estimation device 10.
  • Processor 1040 reads each of these program modules into memory 1060 and executes them, thereby realizing the functions corresponding to each program module.
  • the first acquisition unit 120 acquires point cloud data generated by the lidar.
  • the first acquisition unit 120 may acquire point cloud data generated by the lidar and temporarily stored in the storage unit, or may acquire point cloud data directly from the lidar.
  • the first acquisition unit 120 acquires point cloud data of a plurality of frames in the order in which they are generated.
  • the first area data extraction unit 110 extracts data points within the first area 41 from the point cloud data acquired by the first acquisition unit 120. Subsequent processing is performed on the extracted data points within the first region 41.
  • the position and size of the first region 41 are determined in advance with respect to the moving body 20 as a region where the white line 30 is likely to exist.
  • the first regions 41 are provided at each of the left and right sides of the front and rear sides of the moving body 20, that is, four locations in total are provided. Processing is performed for each first area 41.
  • each data point included in the point cloud data acquired by the first acquisition unit 120 is associated with the intensity of the reflected light received by the lidar.
  • the first estimation unit 180 estimates the position of the white line using the intensities of the plurality of data points in the superimposed point group data.
  • the intensity correction unit 150 corrects the intensity of each data point included in the point group data or superimposed point group data using the distance of the data point.
  • the first estimation unit 180 estimates the position of the white line using the corrected intensity.
  • FIG. 8 is a diagram showing the relationship between reflection intensity and distance before and after correction.
  • the upper graph in this figure shows the relationship between reflection intensity and distance before correction
  • the lower graph shows the relationship between reflection intensity and distance after correction.
  • the white line drawn with retroreflective material has high reflection intensity. Therefore, by selecting data with high reflection intensity within the first region 41, points on the white line can be extracted.
  • the intensity of the light used for measurements by lidar decreases as the distance increases. Specifically, the intensity of reflected light is inversely proportional to the square of the distance. Therefore, as shown in the graph on the upper side of the figure, it is difficult to extract all the point cloud data within the first region 41 using a constant reflection intensity threshold. Even if it were possible to extract it, it would not be easy to set the threshold value.
  • the intensity correction unit 150 obtains information on the extracted data points within the first area 41 from the first area data extraction unit 110, and corrects the reflection intensity as described above.
  • FIGS. 9 and 10 are diagrams for explaining superposition of point cloud data.
  • both white circles and black circles are data points, indicating that the light reflection intensity is higher in the black circles than in the white circles.
  • FIG. 9 shows a case where the end point of the white line 30 enters the first region 41.
  • FIG. 10 shows a case where the end point of the white line 30 deviates from the first area 41.
  • the position estimating device 10 includes a shift amount identifying section that identifies a shift amount using movement information.
  • the coordinate transformation section 163 functions as a shift amount specifying section.
  • the superimposing unit 160 generates superimposed point group data using the shift amount.
  • the movement information is information regarding the movement of the moving object between the timings at which a plurality of point cloud data superimposed in the superimposed point cloud data are acquired.
  • the superimposition unit 160 superimposes point group data at a plurality of times to generate superimposed point group data.
  • coordinate transformation of the point group is performed according to the amount of movement and change in attitude of the vehicle.
  • the details are as follows.
  • the superimposed point group data generated by the superimposed point group data generation section 161 is temporarily stored in the storage section 165.
  • the storage unit 165 is a memory, and is realized by the memory 1060 of the computer 1000.
  • the coordinate transformation unit 163 reads the previous superimposed point group data from the storage unit 165 and performs coordinate transformation on the data.
  • the superimposed point cloud data generation section 161 acquires point cloud data in which the corrected reflection intensity is linked to each point from the intensity correction section 150.
  • the superimposed point group data generation unit 161 superimposes the latest point group data on the superimposed point group data obtained from the coordinate transformation unit 163 and after the coordinate transformation.
  • the latest superimposed point group data thus obtained is output to the first estimation section 180 and is also stored in the storage section 165.
  • the coordinate transformation unit 163 performs coordinate transformation using the movement information acquired by the second acquisition unit 140.
  • the movement information includes at least the velocity v and the yaw angular velocity ⁇ ' of the moving body 20.
  • the movement information may further include roll angular velocity ⁇ ' and pitch angular velocity ⁇ '.
  • the second acquisition unit 140 can acquire movement information from a speed sensor and an angular velocity sensor provided on the moving body 20. Alternatively, the second acquisition unit 140 may calculate the velocity v from the estimated change in self-position over time.
  • the yaw angular velocity ⁇ ' may be the yaw rate output of an IMU (Inertial Measurement Unit) mounted on the moving body 20, or may be derived from the temporal change in the yaw angle of the self-position estimation result.
  • IMU Inertial Measurement Unit
  • FIG. 11 is a diagram for explaining a method in which the coordinate transformation unit 163 performs coordinate transformation of point group data using velocity v and yaw angular velocity ⁇ '.
  • the point group data at the previous time is coordinate-transformed according to the amount of movement and change in posture.
  • the point group data can be expressed as data in the moving body coordinate system after movement.
  • coordinate transformation can be performed according to the following equations (1) and (2).
  • the time interval is ⁇ t
  • the point group data from the time before the previous time can also be expressed in the current moving body coordinate system.
  • the coordinate transformation unit 163 can superimpose the necessary amount of past point cloud data by performing coordinate transformation each time using the movement amount and posture change from the previous time.
  • FIG. 12 is a diagram for explaining a method in which the coordinate transformation unit 163 performs coordinate transformation of point group data using velocity v, yaw angular velocity ⁇ ', roll angular velocity ⁇ ', and pitch angular velocity ⁇ '. If the roll angular velocity ⁇ ' and the pitch angular velocity ⁇ ' can be determined, more accurate point cloud superposition can be achieved by performing coordinate transformation taking these into account. As shown in this figure, if the roll angle change around the x-axis is ⁇ , the pitch angle change around the y-axis is ⁇ , and the yaw angle change around the z-axis is ⁇ , then using three rotation matrices, the following Coordinate transformation can be performed using equations (3) and (4).
  • the x-axis is the traveling direction of the moving body 20
  • the z-axis is a vertically upward direction
  • the y-axis is a direction perpendicular to the x-axis and the z-axis.
  • the second acquisition unit 140 can acquire the roll angular velocity ⁇ ' and the pitch angular velocity ⁇ ' from the roll rate output and pitch rate output of the IMU mounted on the moving body 20, respectively.
  • FIG. 13 is a diagram for explaining the control of the number of times of superimposition.
  • the superimposing unit 160 generates superimposed point group data in which point group data is superimposed a number of times according to the speed of the moving body 20. That is, point cloud data of a number (number of frames) corresponding to the speed of the moving object 20 are superimposed.
  • the number of times of superimposition increases, the number of point group data increases, so the number of points on the white line 30 also increases, making detection easier.
  • the number of times of superimposition is too large, the end points of the broken line cannot be recognized.
  • FIG. 14 is a diagram for explaining extraction of high reflection intensity points.
  • both white circles and black circles are data points, indicating that the light reflection intensity is higher in the black circles than in the white circles.
  • the high intensity point extraction unit 181 extracts high intensity points in the superimposed point group data. Specifically, the high intensity point extraction unit 181 extracts data points having an intensity exceeding a predetermined threshold value from the superimposed point group data.
  • the second specifying unit 189 of the first estimating unit 180 estimates the position of the end point of the line on the road surface.
  • the end point position is a position in the x-axis direction (x coordinate), and is the position of the end of the white line 30 projected on the x-axis.
  • the second specifying unit 189 specifies the position of the minimum or maximum x-coordinate among the x-coordinates of the high-intensity points extracted by the high-intensity point extracting unit 181 as the end point position.
  • the second specifying unit 189 determines whether the specified end point position is within the predetermined second area 42 or not.
  • the second specifying unit 189 takes it as the estimation result of the end point position of the broken line of the white line 30.
  • the estimation result of this end point position is used in the second estimating section 190 to estimate the position of the moving body 20.
  • the second specifying unit 189 does not take it as the estimation result of the end point position of the broken line of the white line 30. That is, when the end point position is within the predetermined second region 42 in the superimposed point group data, the second specifying unit 189 estimates the end point position using the superimposed point group data.
  • the second area 42 is an area including the center of the superimposed point group data in the x-axis direction, and is an area where all the superimposed point group data overlap.
  • the width of the second region 42 in the x-axis direction can be determined by coordinate transformation of the position of the first region 41 used to generate the superimposed point group data. That is, in the cases of FIGS. 9 and 10, in the right side diagram after coordinate transformation, the x coordinate of the front end position of the first region 41 at time t(k-3) and the first region 41 at time t(k) It is calculated as the distance interval from the x-coordinate of the rear end position.
  • the coordinate transformation of each position in the first area 41 is performed using equations (1) and (2), or equations (3) and (4) for the coordinates of the area, similar to the point cloud data. It will be done.
  • FIG. 15 is a diagram for explaining the meaning of the second area 42.
  • both white circles and black circles are data points, indicating that the light reflection intensity is higher in the black circles than in the white circles. If the data in the first area 41 does not capture the end points of the broken line, there will be less data at the end points even if they are superimposed.
  • only the upper two pieces of data have correct end point position data. That is, when the minimum or maximum x-coordinate of the x-coordinates of the high reflection intensity points is far from the center of the superimposed point group data, it is preferable that it is determined to be inappropriate as end point information and not adopted. By doing so, highly accurate end point detection becomes possible.
  • the end point position is determined as the estimation result of the broken line of the white line 30.
  • data can also be used to calculate the lateral position and direction of the white line 30, which will be described later.
  • the principal component analysis section 183 of the first estimation section 180 performs principal component analysis on the high intensity point group extracted by the high intensity point extraction section 181, and obtains the eigenvalue and eigenvector of each principal component axis.
  • the first specifying unit 185 specifies the eigenvector of the principal component axis having the largest eigenvalue as the direction vector of a straight line indicating the direction (longitudinal direction) of the white line 30.
  • FIG. 16 is a diagram for explaining principal component analysis of a point cloud.
  • the principal component analysis unit 183 uses the N points [x i y i z i ] T extracted by the high intensity point extraction unit 181 to calculate the covariance matrix C using the following equation (5) and equation ( Calculate as shown in 6).
  • the eigenvalue can be obtained, and the eigenvector can be calculated from the eigenvalue.
  • ⁇ 1 , ⁇ 2 , and ⁇ 3 be the eigenvalues that are the three solutions of this equation in descending order, and these mean the dispersion of the first to third principal component axes.
  • v 1 , v 2 , and v 3 obtained by substituting each of them into equation (7) are vectors of the first to third principal component axes.
  • the first specifying unit 185 specifies v 1 , which is the vector of the first principal component axis, as a vector in the longitudinal direction of the white line 30. Further, the first identifying unit 185 uses atan2( v 1y , v 1x ), the direction of the white line 30 is specified.
  • v 2 which is a vector of the second principal component axis
  • v 3 which is a vector of the third principal component axis
  • ⁇ 1 , ⁇ 2 , and ⁇ 3 correspond to the dispersion of the point group in the longitudinal direction of the white line 30, the dispersion of the point group in the width direction of the white line 30, and the dispersion of the point group on the surface of the white line 30, respectively.
  • the first specifying unit 185 specifies the position in the y-axis direction of the center of gravity of the point group extracted by the high-intensity point extracting unit 181 as the horizontal position of the white line 30.
  • FIG. 17 and FIG. 18 are diagrams for explaining the detected value and reliability information of the white line 30.
  • the reliability information generation unit 187 uses the results of the principal component analysis to calculate each detection reliability as follows.
  • the specification is made such that r Ly becomes larger as the value increases.
  • r Ly can be set to a value of 0 or more and 1 or less.
  • b is a predetermined coefficient that is the sensitivity to r Ly with respect to
  • the reliability information generation unit 187 specifies the direction detection reliability r L ⁇ such that the larger ⁇ 1 is, the larger r L ⁇ is.
  • the direction is the longitudinal direction of the white line 30.
  • r L ⁇ can be set to a value of 0 or more and 1 or less.
  • c is a predetermined coefficient that is sensitive to r L ⁇ with respect to ⁇ 1 , and may be adjusted according to the resolution of the sensor.
  • the second estimation unit 190 acquires information indicating the end point position of the white line 30 from the second specifying unit 189, and acquires information indicating the direction of the white line 30 and information indicating the lateral position of the white line 30 from the first specifying unit 185.
  • the end point detection reliability r Lx , the lateral position detection reliability r Ly , and the orientation detection reliability r L ⁇ are obtained from the reliability information generation unit 187.
  • the end point detection reliability r Lx , the lateral position detection reliability r Ly , and the orientation detection reliability r L ⁇ are collectively referred to as reliability information.
  • the second estimation unit 190 then performs extended Kalman filter processing using the acquired information to correct the position of the moving body 20.
  • the second estimation unit 190 generates an observation noise matrix R(t) shown in the following equation (9) using the measurement accuracy and reliability information of the white line 30.
  • ⁇ Lx (k) is the measurement accuracy of the distance to the end point position of the white line
  • ⁇ Ly (k) is the measurement accuracy of the lateral distance of the white line
  • ⁇ L ⁇ (k) is the measurement accuracy of the distance to the end point position of the white line 30. This is the measurement accuracy of orientation. Since the accuracy of lidar measurement values deteriorates in proportion to the square of the distance, ⁇ Lx (k), ⁇ Ly (k), and ⁇ L ⁇ (k) are each set as values proportional to the square of the distance. However, if the first area 41 is not very large, it can be set to a predetermined value.
  • each reliability information is multiplied by the reciprocal, so the larger each reliability value (closer to 1), the smaller each element of the noise matrix becomes; (the closer it is to 0), the larger each element of the noise matrix becomes. Note that when the reliability is 0, a value such as 0.0001 is used instead of the reliability.
  • FIG. 19 is a diagram illustrating the positional relationship between the moving body 20 and the white line 30.
  • x w y w is a world coordinate system
  • x b y b is a moving object coordinate system with the moving object 20 as a reference.
  • the xb - axis direction is the traveling direction of the moving body 20.
  • the second estimation unit 190 calculates the measured predicted value of the white line 30, as described above with respect to S10 to S50 in FIG.
  • the measured predicted value is expressed by a matrix L-(k) shown in equation (11) below.
  • x-(k) and y-(k) are the x-coordinate and y-coordinate of the moving body 20 (predicted self-position), respectively, in the world coordinate system
  • ⁇ -(k) is the moving body 20 (predicted self-position) in the world coordinate system. This is the angle of the traveling direction of the predicted self-position with respect to the x and w axes.
  • M x , M y , and M ⁇ are values expressed in the world coordinate system of information about the white line 30 acquired from the landmark map.
  • the white line information stored in the landmark map is shown as a discrete point sequence consisting of latitude, longitude, and altitude information
  • the white line information is displayed in the UTM (Universal Transverse Mercator) coordinate system or plane rectangular coordinate system. Convert to world coordinate system such as system.
  • UTM Universal Transverse Mercator
  • M x is the xw coordinate (end point position) of the end point of the dot sequence of the white line 30
  • M y is the average value of the y w coordinate of the dot sequence of the white line 30 existing in the first area 41
  • M ⁇ is This is the angle of the white line 30 calculated as the direction based on each coordinate of the point sequence of the white line 30.
  • the second estimation unit 190 corrects the predicted self-position using the following equation (12), and sets the corrected predicted self-position as the estimated self-position.
  • the Kalman gain K(k) is the combination of the observed noise matrix R(k) mentioned above, the Jacobian matrix H(k) of 3 rows and 3 columns for the measured predicted value L-(k), and the 3 rows and 3 columns of the Jacobian matrix H(k). It is expressed by the following equation (13) using the dispersion matrix P-(k).
  • Expression (13) is expressed as a fraction, the observation noise matrix R(k) is located in the denominator, so it can be seen that the larger the observation noise matrix R(k) is, the smaller the Kalman gain K(k) is.
  • the reliability information when the reliability information is low, the observation noise matrix R(k) becomes large, the Kalman gain K(k) becomes small, and as a result, the amount of correction for the predicted self-position in equation (12) becomes small. Conversely, when the reliability information is high, the observation noise matrix R(k) does not become large and the Kalman gain K(k) does not become small, and as a result, the amount of correction for the predicted self-position in equation (12) is not appropriate. Become something. As a result, inappropriate correction is suppressed when a measurement value with poor accuracy is obtained, and appropriate correction is performed when a measurement value with high accuracy is obtained, so the accuracy of self-position estimation is always maintained. It is possible to increase it.
  • FIG. 20 is a flowchart illustrating the process flow of S70 shown in FIG. 6.
  • the first area data extraction unit 110 extracts the point cloud data within the first area 41 in S701.
  • the intensity correction unit 150 corrects the reflection intensity of each data point using the distance measurement value of each data point.
  • the superimposing unit 160 generates superimposed point group data.
  • the coordinate conversion unit 163 calculates the number of superimpositions C using the length L of the first region 41, the speed v of the moving body 20, and the frame period T of the rider. Then, the coordinate conversion unit 163 reads out the superimposed point group data held in the storage unit 165, and excludes data from the past C times or earlier.
  • the superimposed point group data generation unit 161 superimposes the latest point group data on the coordinate-transformed superimposed data to generate superimposed point group data.
  • the generated superimposed point group data is passed to the first estimation unit 180 and is held in the storage unit 165.
  • the high intensity point extraction unit 181 extracts data points with high reflection intensity from the superimposed point group data using a threshold value.
  • the second specifying unit 189 uses the x-coordinate as the estimation result of the end point position of the white line 30. do. Furthermore, the reliability information generation unit 187 calculates the endpoint detection reliability.
  • the principal component analysis unit 183 performs principal component analysis on the high reflection intensity point group and calculates the eigenvalue and eigenvector of each principal component axis.
  • the first identifying unit 185 then calculates the lateral position of the white line 30 based on the center of gravity of the high reflection intensity point group, and calculates the direction of the white line 30 from the eigenvector of the first principal component.
  • the reliability information generation unit 187 calculates the lateral position detection reliability using the eigenvalue of the second principal component and the width of the white line 30 read from the landmark map, and calculates the orientation detection reliability from the eigenvalue of the first principal component. Calculate degree.
  • Position estimation device 20 Mobile object 30 White line 40 Measurement area 41 First area 42 Second area 110 First area data extraction unit 120 First acquisition unit 140 Second acquisition unit 150 Intensity correction unit 160 Superimposition unit 161 Superimposed point group data Generation unit 163 Coordinate conversion unit 165 Storage unit 180 First estimation unit 181 High intensity point extraction unit 183 Principal component analysis unit 185 First identification unit 187 Reliability information generation unit 189 Second identification unit 190 Second estimation unit 1000 Computer

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

位置推定装置（１０）は、第１取得部（１２０）、第２取得部（１４０）、重畳部（１６０）、および第１推定部（１８０）を備える。第１取得部（１２０）は、移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する。第２取得部（１４０）は、移動体の移動情報を取得する。重畳部（１６０）は、複数タイミングの点群データが、移動情報に基づいて重ね合わされた、重畳点群データを生成する。第１推定部（１８０）は、重畳点群データを用いて対象物の位置を推定する。

Description

位置推定装置、位置推定方法、およびプログラム
　本発明は、位置推定装置、位置推定方法、およびプログラムに関する。
　車両の周囲に設置されている地物をレーダやカメラ等により検出し、その検出結果を用いて自車位置を推定する技術が知られている。
　特許文献１および２には、道路に設けられた白線などの区画線を検出した結果を用いて、自車位置を推定する方法が開示されている。
国際公開第２０１８／２１２３０２号 国際公開第２０１９／１８９０９８号
　しかしながら、特許文献１および２の技術では、推定精度は区画線を検出したデータ点の粗密に強く依存していた。すなわち、データ点が疎である場合、位置推定精度が低下するものであった。
　本発明が解決しようとする課題としては、データ点が疎であっても精度良く対象物の位置を推定できる技術を提供することが一例として挙げられる。
　請求項１に記載の発明は、
　移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する第１取得部と、
　前記移動体の移動情報を取得する第２取得部と、
　前記複数タイミングの点群データが、前記移動情報に基づいて重ね合わされた、重畳点群データを生成する重畳部と、
　前記重畳点群データを用いて対象物の位置を推定する第１推定部とを備える、
位置推定装置である。
　請求項１２に記載の発明は、
　コンピュータにより実行される位置推定方法であって、
　移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する第１取得ステップと、
　前記移動体の移動情報を取得する第２取得ステップと、
　前記複数タイミングの点群データが、前記移動情報に基づいて重ね合わされた、重畳点群データを生成する重畳ステップと、
　前記重畳点群データを用いて対象物の位置を推定する第１推定ステップとを含む、
位置推定方法である。
　請求項１３に記載の発明は、
　請求項１２に記載の位置推定方法をコンピュータに実行させるプログラムである。
実施形態に係る位置推定装置の構成を例示する図である。 移動体が道路上の白線を検出する様子を例示する図である。 （ａ）および（ｂ）は、それぞれセンサの分解能が高い場合と低い場合の第１領域内のデータ点を示す図である。 実施形態に係る位置推定装置により行われる処理の流れを例示するフローチャートである。 実施例に係る位置推定装置の構成を例示するブロック図である。 実施例に係る位置推定装置が行う自己位置推定処理の流れを例示するフローチャートである。 位置推定装置を実現するための計算機を例示する図である。 補正前後の反射強度と距離との関係を示す図である。 点群データの重畳について説明するための図である。 点群データの重畳について説明するための図である。 座標変換部が速度ｖ、およびヨー角速度ψ'を用いて点群データの座標変換を行う方法について説明するための図である。 座標変換部が速度ｖ、ヨー角速度ψ'、ロール角速度φ'およびピッチ角速度θ'を用いて点群データの座標変換を行う方法について説明するための図である。 重畳回数制御について説明するための図である。 高反射強度点の抽出について説明するための図である。 第２領域の意味について説明するための図である。 点群の主成分分析について説明するための図である。 白線の検出値および信頼度情報について説明するための図である。 白線の検出値および信頼度情報について説明するための図である。 移動体と白線との位置関係を例示する図である。 図６に示したＳ７０の処理の流れを例示するフローチャートである。
　以下、本発明の実施の形態について、図面を用いて説明する。尚、すべての図面において、同様な構成要素には同様の符号を付し、適宜説明を省略する。また、任意の記号の上に「・」または「－」が付された文字を便宜上それぞれ「Ａ'」または「Ａ－」（「Ａ」は任意の文字）と表す。
（実施形態）
　図１は、実施形態に係る位置推定装置１０の構成を例示する図である。本実施形態に係る位置推定装置１０は、第１取得部１２０、第２取得部１４０、重畳部１６０、および第１推定部１８０を備える。第１取得部１２０は、移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する。第２取得部１４０は、移動体の移動情報を取得する。重畳部１６０は、複数タイミングの点群データが、移動情報に基づいて重ね合わされた、重畳点群データを生成する。第１推定部１８０は、重畳点群データを用いて対象物の位置を推定する。なお、複数タイミングの点群データとはたとえば複数の時刻の点群データである。
　車両等の移動体においては、経路のナビゲーションや自動運転、運転補助等を精度良く機能させるために、その移動体の位置を正確に特定することが重要である。移動体では道路を走行する際、ある時点における移動体の位置と、次の時点までの移動体の移動速度や移動方向を用いて次の時点の移動体の位置を推定することが行われる。ここで、移動体の周囲の物体（対象物）をセンサ等で検出し、地図情報におけるその物体の位置と照らし合わせることで、移動体の位置の推定精度をさらに高める補正が可能である。ただし移動体の推定精度を高めるためには、センサによる検出結果を用いて、対象物の位置を高精度に推定する必要がある。
　本実施形態において、位置を推定する対象物は路面上の線である。具体的には、対象物は路面上の区画線であり、破線である。対象物である線の色は特に限定されないが、たとえば黄色または白である。対象物の表面は再帰性反射材により形成されていることが好ましい。
　図２は、移動体２０が道路上の白線３０を検出する様子を例示する図である。移動体２０に搭載されたセンサは、測定領域４０の内側の領域を測定する。中でも位置推定装置１０は詳しく後述するように、第１領域４１内の測定データを抽出して白線３０の検出に利用する。
　本実施形態において、センサは、光を出射し、対象物で反射された反射光を受光することで対象物までの距離を測定する。センサは特に限定されないが、レーダまたはライダ（ＬＩＤＡＲ：Laser Imaging Detection and Ranging， Laser Illuminated Detection and Ranging またはＬｉＤＡＲ：Light Detection and Ranging）等である。センサから出射される光は特に限定されないが、たとえば赤外光である。また、センサから出射される光はたとえばレーザパルスである。センサではたとえばパルス光を出射してからその反射光を受光するまでの時間と、パルス光の伝搬速度とを用いて、センサから対象物までの距離を算出する。センサからの光の出射方向は可変であり、複数の出射方向に対して順に測定を行うことで、測定領域４０内が走査される。
　センサからは、光の反射点の３次元位置と反射強度（すなわちセンサにおける受光強度）とが関連付けられた点群データが出力される。位置推定装置１０の第１取得部１２０はその点群データを取得する。センサから出力される点群データはフレーム単位で構成されている。一つのフレームは、測定領域４０を一度走査することにより得られるデータで構成される。センサでは、測定領域４０内を繰り返し走査することで、連続する複数のフレームを生成する。例えば、出射光を横方向に往復しながら縦方向に移動させることで、それぞれの角度方向に存在する対象物までの距離が計測できるため、横と縦のスキャン範囲内の３次元情報のデータを得ることができる。
　図３（ａ）および図３（ｂ）は、それぞれセンサの分解能が高い場合と低い場合の第１領域４１内のデータ点を示す図である。これらの各図において、白い丸印および黒い丸印はいずれもデータ点であり、白い丸印よりも黒い丸印において光の反射強度が高いことを示している。また、推定される白線３０の方向のイメージを破線矢印で示している。たとえば白線３０は再帰性反射材で描かれているため、周りの路面よりも光の反射率が高く、反射強度が高くなる。よって、測定領域４０の中において、白線３０を検出しやすい場所に第１領域４１を設け、その中で反射強度が高い部分を検出して白線３０を認識する。
　センサの分解能が高い場合には、図３（ａ）に示すように、白線３０の端部の位置および、白線３０の方向が明確である。白線３０の長手方向を縦方向、幅方向を横方向とすると、横方向の１ラインごとに複数の白線３０上の点が得られるため、白線３０を検出した両端の点間隔から白線３０の幅と同等かを判定し、ラインごとに中心点を求め、それらを結ぶことで白線３０の中心線を求めることができる。また、白線３０上の点を検出したラインと検出しなかったラインとの間に破線の端点があると判断することができる。
　一方、図３（ｂ）に示すように、センサの分解能が低い場合には、一つのフレームのデータのみからは、白線３０の端部の位置や白線３０の方向を明確に特定することができない。まず、取得された点群が疎であるため、白線３０の幅との比較により白線３０上の点であるか否かの判定が難しい。たとえば白線３０上の点であると判定できたとしても、ライン毎の平均位置を正確に求められないため、中心線が生成できない。また、白線３０を検出したラインと検出しなかったラインとのコントラストが低いため、どこが破線の端点なのか分からない。
　本実施形態に係る位置推定装置１０によれば、重畳部１６０は、複数タイミングの点群データが、移動情報に基づいて重ね合わされた、重畳点群データを生成する。そして第１推定部１８０は、重畳点群データを用いて対象物の位置を推定する。したがって、各フレームで得られるデータ点が疎であっても、データ点が密である重畳点群データを生成し、精度良く道路上の線等の対象物の位置を推定できる。
　図４は、本実施形態に係る位置推定装置１０により行われる処理の流れを例示するフローチャートである。本実施形態に係る位置推定方法は、コンピュータにより実行される。本実施形態に係る位置推定方法は、第１取得ステップＳ１０１と、第２取得ステップＳ１０２と、重畳ステップＳ１０３と、第１推定ステップＳ１０４とを含む。第１取得ステップＳ１０１では、移動体に搭載されたセンサで得られた、複数タイミングの点群データが取得される。第２取得ステップＳ１０２では、移動体の移動情報が取得される。重畳ステップＳ１０３では、複数タイミングの点群データが、移動情報に基づいて重ね合わされた、重畳点群データが生成される。第１推定ステップＳ１０４では、重畳点群データを用いて対象物の位置が推定される。
（実施例）
　図５は、実施例に係る位置推定装置１０の構成を例示するブロック図である。本実施例に係る位置推定装置１０は、実施形態に係る位置推定装置１０の構成を有している。本実施例に係る位置推定装置１０は、第１領域内データ抽出部１１０、強度補正部１５０、第２推定部１９０をさらに備える。本実施例に係る重畳部１６０は、重畳点群データ生成部１６１、座標変換部１６３、および記憶部１６５を備える。本実施例に係る第１推定部１８０は、高強度点抽出部１８１、主成分分析部１８３、第１特定部１８５、信頼度情報生成部１８７、および第２特定部１８９を備える。以下に詳しく説明する。
　図６は、実施例に係る位置推定装置１０が行う自己位置推定処理の流れを例示するフローチャートである。第２推定部１９０は、第１推定部１８０による対象物の位置の推定結果を用いて、移動体２０の位置を推定する。本図を参照し、位置推定装置１０にて行われる移動体２０の位置推定のための処理の全体の流れを説明する。以下、対象物を白線として説明するが、対象物は白線以外であってもよい。また、センサをライダとして説明するが、センサは他の方式であっても良い。
　位置推定装置１０の動作が開始されると、位置推定装置１０はまず初回の推定自己位置を特定する（Ｓ１０）。具体的には、第２推定部１９０は移動体２０または位置推定装置１０に設けられたＧＮＳＳ（Global Navigation Satellite System）による測位結果を初回の推定自己位置とする。次いで、第２推定部１９０は前回の推定自己位置を用いて最新の予測自己位置を算出する（Ｓ２０）。具体的には第２推定部１９０は、移動体２０に設けられた速度センサとジャイロセンサから、移動体２０の速度とヨー角速度を取得し、前回の推定自己位置からの移動体２０の移動方向および移動量を特定する。そして、第２推定部１９０は、前回の推定自己位置に対して、特定した移動方向および移動量の移動を行った場合の移動後の位置を、予測自己位置として算出する。
　次いで、第２推定部１９０は、予測自己位置周辺の地図が既に取得済みであるか否かを判定する（Ｓ３０）。その地図が既に取得済みである場合（Ｓ３０のＹｅｓ）、第２推定部１９０はＳ４０の処理を行わずにＳ５０の処理を行う。その地図が未だ取得されていない場合（Ｓ３０のＮｏ）、第２推定部１９０は、予測自己位置周辺のランドマーク地図を取得する（Ｓ４０）。第２推定部１９０はランドマーク地図を、第２推定部１９０からアクセス可能な記憶部から読み出して取得しても良いし、ネットワークを通じて外部から取得しても良い。第２推定部１９０からアクセス可能な記憶部は位置推定装置１０の内部に設けられていても良いし、位置推定装置１０の外部に設けられていても良い。
　次いで第２推定部１９０は、移動体２０からライダで検出可能な距離にある白線の情報を地図から取得し、白線の計測予測値を算出する（Ｓ５０）。計測予測値とは、その白線が、移動体２０から見てどのように測定されるかの予測値である。具体的には第２推定部１９０は、ランドマーク地図のうち、移動体２０から検出可能な距離にある白線の情報を特定する。そして、ランドマーク地図に示されたその白線の位置および向きと、予測自己位置とに基づいて、その白線の計測予測値を算出する。
　次いで、第１取得部１２０は、点群データを取得できたか否かを判定する（Ｓ６０）。他車両等によるオクルージョンが発生した場合など、点群データが取得されない場合（Ｓ６０のＮｏ）、処理はＳ２０に戻る。第１取得部１２０で点群データが取得された場合（Ｓ６０のＹｅｓ）、Ｓ７０において点群重畳による白線検出と、信頼度情報の生成が行われる。Ｓ７０については図２０を参照して詳しく後述する。
　Ｓ７０において白線の位置および方向を示す情報と、信頼度情報が生成されると、第２推定部１９０は、それらの情報を用いてＳ２０で算出された予測自己位置を補正する（Ｓ８０）。予測自己位置を補正に用いる手法は特に限定されないが、たとえば拡張カルマンフィルタ処理を利用することができる。そして第２推定部１９０は、補正後の予測自己位置を最新の推定自己位置とする。推定自己位置は、位置推定装置１０以外の装置に対して出力されても良いし、第２推定部１９０からアクセス可能な記憶装置に保持されても良い。推定自己位置は、経路等のナビゲーション、自動運転、または運転補助等の機能のために用いられうる。
　次いで第２推定部１９０は終了条件が満たされているか否かを判定する（Ｓ９０）。終了条件が満たされている場合とはたとえば、移動体２０の動作が停止した場合や、位置推定装置１０による推定処理を停止させる操作が行われた場合である。終了条件が満たされている場合（Ｓ９０のＹｅｓ）、位置推定装置１０は処理を終了する。終了条件が満たされていない場合（Ｓ９０のＮｏ）、処理はＳ２０に戻る。
　位置推定装置１０のハードウエア構成について以下に説明する。位置推定装置１０の各機能構成部は、各機能構成部を実現するハードウエア（例：ハードワイヤードされた電子回路など）で実現されてもよいし、ハードウエアとソフトウエアとの組み合わせ（例：電子回路とそれを制御するプログラムの組み合わせなど）で実現されてもよい。以下、位置推定装置１０の各機能構成部がハードウエアとソフトウエアとの組み合わせで実現される場合について、さらに説明する。
　図７は、位置推定装置１０を実現するための計算機１０００を例示する図である。計算機１０００は任意の計算機である。例えば計算機１０００は、SoC（System On Chip）、Personal Computer（PC）、サーバマシン、タブレット端末、又はスマートフォンなどである。計算機１０００は、位置推定装置１０を実現するために設計された専用の計算機であってもよいし、汎用の計算機であってもよい。
　計算機１０００は、バス１０２０、プロセッサ１０４０、メモリ１０６０、ストレージデバイス１０８０、入出力インタフェース１１００、及びネットワークインタフェース１１２０を有する。バス１０２０は、プロセッサ１０４０、メモリ１０６０、ストレージデバイス１０８０、入出力インタフェース１１００、及びネットワークインタフェース１１２０が、相互にデータを送受信するためのデータ伝送路である。ただし、プロセッサ１０４０などを互いに接続する方法は、バス接続に限定されない。プロセッサ１０４０は、CPU（Central Processing Unit）、GPU（Graphics Processing Unit）、又は FPGA（Field-Programmable Gate Array）などの種々のプロセッサである。メモリ１０６０は、RAM（Random Access Memory）などを用いて実現される主記憶装置である。ストレージデバイス１０８０は、ハードディスク、SSD（Solid State Drive）、メモリカード、又は ROM（Read Only Memory）などを用いて実現される補助記憶装置である。
　入出力インタフェース１１００は、計算機１０００と入出力デバイスとを接続するためのインタフェースである。例えば入出力インタフェース１１００には、キーボードなどの入力装置や、ディスプレイなどの出力装置が接続される。
　ネットワークインタフェース１１２０は、計算機１０００をネットワークに接続するためのインタフェースである。この通信網は、例えば LAN（Local Area Network）や WAN（Wide Area Network）である。ネットワークインタフェース１１２０がネットワークに接続する方法は、無線接続であってもよいし、有線接続であってもよい。
　ストレージデバイス１０８０は、位置推定装置１０の各機能構成部を実現するプログラムモジュールを記憶している。プロセッサ１０４０は、これら各プログラムモジュールをメモリ１０６０に読み出して実行することで、各プログラムモジュールに対応する機能を実現する。
　位置推定装置１０の各構成要素について以下に詳しく説明する。第１取得部１２０は、ライダで生成された点群データを取得する。第１取得部１２０は、ライダで生成され、一旦記憶部に保持された点群データを取得しても良いし、ライダから直接点群データを取得しても良い。第１取得部１２０は、複数のフレームの点群データを生成された順に取得する。
＜第１領域内データ点の抽出＞
　第１領域内データ抽出部１１０は、第１取得部１２０で取得された点群データのうち、第１領域４１内のデータ点を抽出する。抽出された第１領域４１内のデータ点に対し、以後の処理が行われる。第１領域４１は、白線３０が存在する可能性が高い領域として、移動体２０を基準に予め位置および大きさが定められている。第１領域４１は、移動体２０の前方と後方のそれぞれ左右に設けられ、すなわち計４箇所設けられる。第１領域４１ごとに、処理が行われる。
＜強度補正＞
　上述した通り、第１取得部１２０が取得した点群データに含まれる各データ点には、ライダで受光した反射光の強度が関連付けられている。そして後述する通り、第１推定部１８０は、重畳点群データにおける複数のデータ点の強度を用いて、白線の位置を推定する。ここで、強度補正部１５０は、点群データまたは重畳点群データに含まれる各データ点の強度を、そのデータ点の距離を用いて補正する。第１推定部１８０は、補正後の強度を用いて白線の位置を推定する。
　図８は、補正前後の反射強度と距離との関係を示す図である。本図の上側のグラフでは補正前の反射強度と距離との関係を示し、下側のグラフでは補正後の反射強度と距離との関係を示している。再帰性反射材で描かれた白線は反射強度が高い。よって、第１領域４１内において反射強度の高いデータを選択することで、白線上の点を抽出することができる。しかし、ライダで測定に利用される光は、距離が遠くなるほど強度が低下する。具体的には、反射光強度は距離の２乗に反比例する。したがって、本図上側のグラフに示すように、第１領域４１内の点群データすべてに対して一定の反射強度閾値を用いて抽出を行うことが難しい。たとえ抽出できたとしても、閾値の設定が容易ではない。そこで、強度補正部１５０は検出点までの距離ｒの２乗値を用いて反射強度Ｉを補正する。具体的には、強度補正部１５０はＩ'＝Ａ×ｒ２×Ｉの関係に基づいて反射強度補正値I'（補正後の反射強度）を算出する。なお、Ａは所定の係数である。この補正により、本図下側のグラフに示すように、検出された強度に対する距離の影響が除かれる。したがって、補正後の強度に対し、一定の閾値による高反射強度点（「高強度点」とも呼ぶ。）の抽出が可能となる。
　強度補正部１５０は、抽出された第１領域４１内のデータ点の情報を第１領域内データ抽出部１１０から取得し、上述した通り反射強度を補正する。
＜点群重畳＞
　図９および図１０は、点群データの重畳について説明するための図である。これらの図において、白い丸印および黒い丸印はいずれもデータ点であり、白い丸印よりも黒い丸印において光の反射強度が高いことを示している。図９は、白線３０の端点が第１領域４１に入って来た場合を示している。図１０は、白線３０の端点が第１領域４１から外れていく場合を示している。位置推定装置１０は、移動情報を用いてずらし量を特定するずらし量特定部を備える。図５の例において、座標変換部１６３がずらし量特定部として機能する。重畳部１６０は、ずらし量を用いて重畳点群データを生成する。ここで移動情報は、重畳点群データにて重畳される複数の点群データが取得されたタイミング間の、移動体の移動に関する情報である。
　重畳部１６０は、複数の時刻の点群データを重畳して重畳点群データを生成する。ここで、重畳に際しては、車両の移動量や姿勢変化に応じて、点群の座標変換を行う。具体的には以下の通りである。重畳点群データ生成部１６１で生成された重畳点群データは一時的に記憶部１６５に保存される。記憶部１６５は、メモリであり、計算機１０００のメモリ１０６０により実現される。座標変換部１６３では、前回の重畳点群データを記憶部１６５から読み出し、そのデータに対して座標変換が行われる。重畳点群データ生成部１６１は強度補正部１５０から補正後の反射強度が各点に紐付けられた点群データを取得する。重畳点群データ生成部１６１は、座標変換部１６３から取得した、座標変換後の重畳点群データに対して最新の点群データを重畳する。そうして得られた最新の重畳点群データは第１推定部１８０に対して出力されるとともに、記憶部１６５に保存される。
＜＜座標変換＞＞
　座標変換部１６３は第２取得部１４０が取得した移動情報を用いて座標変換を行う。移動情報には移動体２０の速度ｖ、ヨー角速度ψ'が少なくとも含まれる。移動情報にはロール角速度φ'およびピッチ角速度θ'がさらに含まれても良い。第２取得部１４０は、移動体２０に設けられた速度センサおよび角速度センサから移動情報を取得することができる。または第２取得部１４０は推定した自己位置の時間変化から速度ｖを算出しても良い。また、ヨー角速度ψ'は移動体２０に搭載したＩＭＵ（Inertial Measurement Unit）のヨーレート出力でもよいし、自己位置推定結果のヨー角の時間変化から導出されても良い。
　図１１は、座標変換部１６３が速度ｖ、およびヨー角速度ψ'を用いて点群データの座標変換を行う方法について説明するための図である。本図のように移動体２０が移動した場合、その移動量や姿勢変化に応じて前回時刻の点群データを座標変換する。そうすることで、点群データを移動後の移動体座標系でのデータとして表すことができる。点ｐを対象にした場合、以下の式（１）および式（２）に従い座標変換を実施できる。ここで、時間間隔をΔｔとすると、移動体２０の進行方向の移動量Δｘと姿勢変化ΔψはそれぞれΔｘ＝ｖΔｔ、およびΔψ＝ψ'Δｔで表すことができる。なお、ロールとピッチの変化は僅かであるため、点群重畳のための座標変換のパラメータとして加えずとも大きな影響はない。
Figure JPOXMLDOC01-appb-M000001
Figure JPOXMLDOC01-appb-M000002
　式（１）の結果を式（２）に代入すれば、前々回時刻の点群データも現在の移動体座標系で表現できる。このとき式（１）の結果を得た時点で、Δψ１およびΔｘ１の情報は必要なくなる事がわかる。すなわち、座標変換部１６３は前回からの移動量と姿勢変化を用いた座標変換を都度行うことで、必要な分の過去の点群データを重畳する事ができる。
　図１２は、座標変換部１６３が速度ｖ、ヨー角速度ψ'、ロール角速度φ'およびピッチ角速度θ'を用いて点群データの座標変換を行う方法について説明するための図である。ロール角速度φ'やピッチ角速度θ'が把握できる場合は、それらも考慮して座標変換を行うことにより、さらに正確な点群重畳が可能となる。本図に示すうように、ｘ軸周りのロール角変化をΔφ、ｙ軸周りのピッチ角変化をΔθ、ｚ軸周りのヨー角変化をΔψとすると、３回の回転行列を用いて、以下の式（３）および式（４）により座標変換が実施できる。ｘ軸は移動体２０の進行方向であり、ｚ軸は鉛直上方向であり、ｙ軸はｘ軸およびｚ軸に垂直な方向である。ここで、移動体２０の速度ｖ、ロール角速度φ'、ピッチ角速度θ'、ヨー角速度ψ'、時間間隔Δｔを用いて、移動量と各姿勢変化はΔｘ＝ｖΔｔ、Δφ＝φ'Δｔ、Δθ＝θ'Δｔ、およびΔψ＝ψ'Δｔで示される。第２取得部１４０は、ロール角速度φ'およびピッチ角速度θ'を、移動体２０に搭載したＩＭＵのロールレート出力およびピッチレート出力からそれぞれ取得することができる。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
＜＜重畳回数制御＞＞
　図１３は、重畳回数制御について説明するための図である。本実施例において重畳部１６０は、移動体２０の速度に応じた回数の点群データが重ね合わされた、重畳点群データを生成する。すなわち、移動体２０の速度に応じた数（フレーム数）の点群データが重ね合わされる。重畳する回数を増やすほど点群データ数が増えるため、白線３０上の点も増加し、より検出が容易となる。一方、重畳回数が多すぎると、破線端点が判らなくなってしまう。白線３０の破線端点を求めるには、重畳点群データ内で高反射強度点の境界が把握できるような状態である必要がある。したがって、正確な端点検出のためには、破線端点の境界が認識できるように重畳する回数の制御することが好ましい。
　たとえば第１領域４１の進行方向の長さをＬとする場合、複数回の点群重畳がＬ／２までの移動の範囲内であれば、重畳される複数の点群データの中に、白線３０の破線部分が第１領域４１内に位置するような点群データが必ず存在する。したがって、座標変換部１６３は第１領域４１の長さＬ、移動体２０の速度ｖ、およびライダのフレーム周期Ｔを用い、ｍ＝（Ｌ／２）／ｖ／Ｔ＝Ｌ／（２ｖＴ）の関係に基づいてｍを算出する。そして、座標変換部１６３は、ｍを超えない最大の整数を重畳回数Ｃとして特定する。たとえばＬ＝１０［ｍ］、ｖ＝２７．８［ｍ／ｓ］（＝１００［ｋｍ／ｈ］）、Ｔ＝４２［ｍｓ］の場合、ｍ＝１０／（２×２７．８×０．０４２）＝４．２８である。したがって、重畳回数Ｃは４回と決定される。
＜高反射強度点抽出と端点検出＞
　図１４は、高反射強度点の抽出について説明するための図である。本図において、白い丸印および黒い丸印はいずれもデータ点であり、白い丸印よりも黒い丸印において光の反射強度が高いことを示している。第１推定部１８０が重畳点群データを重畳点群データ生成部１６１から取得すると、高強度点抽出部１８１が重畳点群データにおける高強度点を抽出する。具体的には高強度点抽出部１８１は、重畳点群データのうち所定の閾値を超える強度を有するデータ点を抽出する。
＜＜端点判定の可否＞＞
　第１推定部１８０の第２特定部１８９は、路面上の線の端点位置を推定する。端点位置は、ｘ軸方向の位置（ｘ座標）であり、ｘ軸に投影された白線３０の、端部の位置である。第２特定部１８９は高強度点抽出部１８１で抽出された高強度点のｘ座標のうち、最小または最大のｘ座標の位置を端点位置として特定する。ここで、第２特定部１８９は、特定した端点位置が所定の第２領域４２内にあるか否かを判定する。特定した端点位置が第２領域４２内にある場合、第２特定部１８９はそれを白線３０の破線の端点位置の推定結果とする。この端点位置の推定結果が第２推定部１９０において移動体２０の位置推定に用いられる。一方、特定した端点位置が第２領域４２内にない場合、第２特定部１８９はそれを白線３０の破線の端点位置の推定結果としない。すなわち、第２特定部１８９は、端点位置が重畳点群データ中の所定の第２領域４２内にある場合に、その重畳点群データを用いて、端点位置を推定する。第２領域４２は、重畳点群データのｘ軸方向の中心を含む領域であり、重畳されたすべての点群データが重なっている領域である。なお、第２領域４２のｘ軸方向の幅は、重畳点群データの生成に用いられた第１領域４１の位置を座標変換することで求めることができる。すなわち、図９および図１０の場合では、座標変換後の右側の図において、時刻ｔ（ｋ－３）の第１領域４１の前端位置のｘ座標と、時刻ｔ（ｋ）の第１領域４１の後端位置のｘ座標との距離間隔として算出される。なお、第１領域４１の各位置の座標変換は、点群データと同様、領域の座標に対して、式（１）および式（２）、あるいは式（３）および式（４）を用いて行われる。
　図１５は、第２領域４２の意味について説明するための図である。本図において、白い丸印および黒い丸印はいずれもデータ点であり、白い丸印よりも黒い丸印において光の反射強度が高いことを示している。第１領域４１内のデータが破線端点を捉えていない場合があると、重畳しても端点位置にあるデータは少なくなってしまう。図１５の例では、上の２つのデータしか正しく端点位置のデータを有していない。すなわち、高反射強度点のｘ座標のうち、最小あるいは最大のｘ座標が、重畳点群データの中央部から離れているときは、端点情報として適切でないと判断して採用しないことが好ましい。そうすることで、高精度な端点検出が可能となる。したがって、重畳点群データのうち所定の閾値を超える強度を有するデータ点の端点が、第２領域４２の中にある場合のみ、その端点位置を白線３０の破線の推定結果とする。ただし、このようなデータでも後述する白線３０の横位置や向きの算出には用いることができる。
＜白線方向の特定＞
　一方、第１推定部１８０の主成分分析部１８３は、高強度点抽出部１８１で抽出された高強度点群に対して、主成分分析を行い、各主成分軸の固有値および固有ベクトルを求める。そして第１特定部１８５は、最も固有値の大きい主成分軸の固有ベクトルを白線３０の方向（長手方向）を示す直線の方向ベクトルとして特定する。
　図１６は点群の主成分分析について説明するための図である。まず、主成分分析部１８３は、高強度点抽出部１８１で抽出されたＮ個の点［ｘｉ　ｙｉ　ｚｉ］Ｔを用いて、共分散行列Ｃを以下の式（５）および式（６）のように算出する。
Figure JPOXMLDOC01-appb-M000005
Figure JPOXMLDOC01-appb-M000006
　この共分散行列Ｃの固有値λと固有ベクトルｖを用いると以下の式（７）が成り立つ。
Figure JPOXMLDOC01-appb-M000007
　ゆえに、以下の特性方程式（８）を解くことにより固有値を求め、その固有値から固有ベクトルを算出することができる。
Figure JPOXMLDOC01-appb-M000008
　この方程式の３つの解である固有値を大きい順にλ１、λ２、およびλ３とすると、それらは第１～第３主成分軸の分散を意味している。また、それらのそれぞれを式（７）に代入して求めたｖ１、ｖ２、およびｖ３は、第１～第３主成分軸のベクトルである。そして、第１特定部１８５は、第１主成分軸のベクトルであるｖ１を白線３０の長手方向のベクトルとして特定する。また、第１特定部１８５は、特定した白線３０の長手方向のベクトルのｘ成分の値ｖ１ｘとｙ成分の値ｖ１ｙを用い、逆正接（アーク・タンジェント）を求める関数であるａｔａｎ２（ｖ１ｙ，ｖ１ｘ）演算により、白線３０の向きを特定する。なお、第２主成分軸のベクトルであるｖ２は白線３０の幅方向のベクトルに相当し、第３主成分軸のベクトルであるｖ３は白線３０面の法線ベクトルに相当する。ここで、λ１、λ２、およびλ３はそれぞれ、白線３０の長手方向の点群の分散、白線３０の幅方向の点群の分散、および白線３０面の点群の分散に相当する。また、第１特定部１８５は、高強度点抽出部１８１で抽出された点群の重心のｙ軸方向の位置を白線３０の横位置として特定する。
＜信頼度情報の生成＞
　図１７および図１８は、白線３０の検出値および信頼度情報について説明するための図である。信頼度情報生成部１８７は主成分分析の結果を用いて、以下のように各検出信頼度を算出する。
　信頼度情報生成部１８７は、端点検出信頼度ｒＬｘを、第２領域４２の中にある高反射強度点の数ｎが大きいほど、ｒＬｘが大きくなるよう特定する。具体的には信頼度情報生成部１８７は、ｒＬｘ＝１－ｅｘｐ（－ａ×ｎ）の関係に基づいて端点検出信頼度ｒＬｘを算出する。この式により、ｒＬｘは０以上１以下の値にすることができる。なお、ａはｎに対するｒＬｘへの感度となる所定の係数であり、センサの分解能に応じて調整されてもよい。
　また信頼度情報生成部１８７は、横位置検出信頼度ｒＬｙを、２×３√λ２（＝±３シグマの範囲）と、ランドマーク地図から取得した白線の幅ＷＭとの差が小さいほど、ｒＬｙが大きくなるよう特定する。具体的には信頼度情報生成部１８７は、ｒＬｙ＝ｅｘｐ（－ｂ×｜２×３√λ２－ＷＭ｜）の関係に基づいて横位置検出信頼度ｒＬｙを算出する。この式により、ｒＬｙは０以上１以下の値にすることができる。なお、ｂは｜２×３√λ２－ＷＭ｜に対するｒＬｙへの感度となる所定の係数であり、センサの分解能に応じて調整されてもよい。
　そして信頼度情報生成部１８７は、向き検出信頼度ｒＬψを、√λ１が大きいほどｒＬψが大きくなるよう特定する。なお、向きとは白線３０の長手方向である。具体的には信頼度情報生成部１８７は、ｒＬψ＝１－ｅｘｐ（－ｃ×√λ１）の関係に基づいて向き検出信頼度ｒＬψを算出する。この式により、ｒＬψは０以上１以下の値にすることができる。なお、ｃは√λ１に対するｒＬψへの感度となる所定の係数であり、センサの分解能に応じて調整されてもよい。
　このように、主成分分析を利用することで、点群データのデータ点の配置に関わらず、すなわち点群が図３（ａ）のようにライン上に並んでおらず、図３（ｂ）のように疎であっても、拡張カルマンフィルタ処理への適用が可能となる。
＜移動体位置の補正＞
　第２推定部１９０は、第２特定部１８９から白線３０の端点位置を示す情報を取得し、第１特定部１８５から白線３０の向きを示す情報および白線３０の横位置を示す情報を取得し、信頼度情報生成部１８７から端点検出信頼度ｒＬｘ、横位置検出信頼度ｒＬｙ、および向き検出信頼度ｒＬψを取得する。端点検出信頼度ｒＬｘ、横位置検出信頼度ｒＬｙ、および向き検出信頼度ｒＬψを合わせて信頼度情報と呼ぶ。そして第２推定部１９０は取得したそれらの情報を用いて拡張カルマンフィルタ処理を実施し、移動体２０の位置を補正する。
＜＜観測雑音行列の生成＞＞
　具体的には第２推定部１９０は、白線３０の計測精度および信頼度情報を用いて以下の式（９）に示す観測雑音行列Ｒ（ｔ）を生成する。
Figure JPOXMLDOC01-appb-M000009
　ここで、σＬｘ（ｋ）は白線３０の端点位置までの距離の計測精度であり、σＬｙ（ｋ）は白線３０の横方向距離の計測精度であり、σＬψ（ｋ）は白線３０の向きの計測精度である。ライダの計測値は距離の２乗に比例して精度が悪化するため、σＬｘ（ｋ）、σＬｙ（ｋ）、およびσＬψ（ｋ）はそれぞれ、距離の２乗に比例した値として設定されてもよいが、第１領域４１があまり大きくない場合等には所定の値とすることができる。式（９）からわかるように、各信頼度情報の逆数が乗じられているため、各信頼度が大きいほど（１に近いほど）雑音行列の各要素は小さくなり、各信頼度が小さいほど（０に近いほど）雑音行列の各要素は大きくなる。なお、信頼度が０の時は０．０００１のような値をその信頼度の代わりに用いる。
＜＜移動体位置の補正＞＞
　図１９は、移動体２０と白線３０との位置関係を例示する図である。本図において、ｘｗｙｗはワールド座標系であり、ｘｂｙｂは移動体２０を基準とした移動体座標系である。ｘｂ軸方向は、移動体２０の進行方向である。第２推定部１９０が取得した白線３０の端点位置Ｌｘ（ｋ）、横位置Ｌｙ（ｋ）、および向きＬψ（ｋ）を用いて、白線３０の計測値は以下の式（１０）に示す行列Ｌ（ｋ）で表される。
Figure JPOXMLDOC01-appb-M000010
　また第２推定部１９０は、図６のＳ１０からＳ５０に関して上述した通り、白線３０の計測予測値を算出する。計測予測値は以下の式（１１）に示す行列Ｌ－（ｋ）で表される。図１９に示す通り、ｘ－（ｋ）およびｙ－（ｋ）はそれぞれワールド座標系における移動体２０（予測自己位置）のｘ座標およびｙ座標であり、ψ－（ｋ）は移動体２０（予測自己位置）の進行方向のｘｗ軸に対する角度である。また、Ｍｘ、Ｍｙ、Ｍψは、ランドマーク地図から取得された白線３０の情報を、ワールド座標系で表現した値である。なお、ランドマーク地図に格納されている白線情報が緯度、経度、高度の情報からなる離散的な点列で示されている場合は、白線情報をＵＴＭ（Universal Transverse Mercator）座標系あるいは平面直角座標系等のワールド座標系に変換する。Ｍｘは白線３０の点列の端点のｘｗ座標（端点位置）であり、Ｍｙは第１領域４１内に存在する白線３０の点列のｙｗ座標の平均値であり、Ｍψは白線３０の点列の各座標を基に向きとして算出した白線３０の角度である。
Figure JPOXMLDOC01-appb-M000011
　そして第２推定部１９０は、以下の式（１２）を用いて予測自己位置を補正し、補正後の予測自己位置を推定自己位置とする。ここで、カルマンゲインＫ（ｋ）は、上述した観測雑音行列Ｒ（ｋ）と、計測予測値Ｌ－（ｋ）に対する３行３列のヤコビ行列Ｈ（ｋ）と、３行３列の共分散行列Ｐ－（ｋ）とを用いて、以下の式（１３）で表される。式（１３）を分数で表すと、観測雑音行列Ｒ（ｋ）は分母に位置するため、観測雑音行列Ｒ（ｋ）が大きいと、カルマンゲインＫ（ｋ）が小さくなることがわかる。すなわち、信頼度情報が低いと、観測雑音行列Ｒ（ｋ）が大きくなり、カルマンゲインＫ（ｋ）が小さくなり、結果として、式（１２）における予測自己位置に対する補正量が少なくなる。逆に、信頼度情報が高いと、観測雑音行列Ｒ（ｋ）が大きくならず、カルマンゲインＫ（ｋ）が小さくならず、結果として、式（１２）における予測自己位置に対する補正量が適切なものとなる。これにより、正確性の乏しい計測値が得られた場合は不適切な補正は抑制され、正確性の高い計測値が得られた場合は適切な補正が行われるため、自己位置推定の精度を常に高めることが可能となる。
Figure JPOXMLDOC01-appb-M000012
Figure JPOXMLDOC01-appb-M000013
　図２０は、図６に示したＳ７０の処理の流れを例示するフローチャートである。第１取得部１２０が点群データを取得すると、Ｓ７０１において第１領域内データ抽出部１１０は第１領域４１内の点群データを抽出する。次いでＳ７０２において強度補正部１５０が各データ点の距離計測値を用いて各データ点の反射強度を補正する。次いでＳ７０３において重畳部１６０が重畳点群データを生成する。具体的には、座標変換部１６３は第１領域４１の長さＬ、移動体２０の速度ｖ、およびライダのフレーム周期Ｔを用いて重畳回数Ｃを算出する。そして座標変換部１６３は記憶部１６５に保持されている重畳点群データを読み出し、過去Ｃ回以前のデータを除外する。そして座標変換部１６３は、残った重畳データ（ただしＣ＝２である場合は、１回分の点群データ。以下同じ。）に対してたとえば移動体２０の速度ｖとヨー角速度ψ'を用いて座標変換を行う。重畳点群データ生成部１６１は、座標変換された重畳データに対して最新の点群データを重畳し、重畳点群データを生成する。生成された重畳点群データは第１推定部１８０に渡されるとともに、記憶部１６５に保持される。
　次いでＳ７０４において、高強度点抽出部１８１が、重畳点群データのうち高反射強度のデータ点を、閾値を用いて抽出する。
　Ｓ７０５において、第２特定部１８９は、高反射強度点のｘ座標のうち、最小あるいは最大のｘ座標が、第２領域４２内にある場合、そのｘ座標を白線３０の端点位置の推定結果とする。また、信頼度情報生成部１８７は端点検出信頼度を算出する。
　Ｓ７０６において、主成分分析部１８３は高反射強度点群に対して主成分分析を行い、各主成分軸の固有値と固有ベクトルを算出する。そして第１特定部１８５は高反射強度点群の重心点に基づいて白線３０の横位置を算出し、第１主成分の固有ベクトルから白線３０の向きを算出する。また、信頼度情報生成部１８７は、第２主成分の固有値とランドマーク地図から読みだした白線３０の幅を用いて横位置検出信頼度を算出し、第１主成分の固有値から向き検出信頼度を算出する。
　以上、本実施形態によれば、第１の実施形態と同様の作用および効果を得られる。
　以上、図面を参照して実施形態及び実施例について述べたが、これらは本発明の例示であり、上記以外の様々な構成を採用することもできる。
　この出願は、２０２２年３月１４日に出願された日本出願特願２０２２－０３９３８８号を基礎とする優先権を主張し、その開示の全てをここに取り込む。
１０　位置推定装置
２０　移動体
３０　白線
４０　測定領域
４１　第１領域
４２　第２領域
１１０　第１領域内データ抽出部
１２０　第１取得部
１４０　第２取得部
１５０　強度補正部
１６０　重畳部
１６１　重畳点群データ生成部
１６３　座標変換部
１６５　記憶部
１８０　第１推定部
１８１　高強度点抽出部
１８３　主成分分析部
１８５　第１特定部
１８７　信頼度情報生成部
１８９　第２特定部
１９０　第２推定部
１０００　計算機

Claims (13)

  1. 　移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する第１取得部と、
    　前記移動体の移動情報を取得する第２取得部と、
    　前記複数タイミングの点群データが、前記移動情報に基づいて重ね合わされた、重畳点群データを生成する重畳部と、
    　前記重畳点群データを用いて対象物の位置を推定する第１推定部とを備える、
    位置推定装置。
  2. 　請求項１に記載の位置推定装置において、
    　前記移動情報を用いてずらし量を特定するずらし量特定部を備え、
    　前記重畳部は、前記ずらし量を用いて前記重畳点群データを生成する
    位置推定装置。
  3. 　請求項１または２に記載の位置推定装置において、
    　前記移動情報は、前記重畳点群データにて重畳される複数の点群データが取得されたタイミング間の、前記移動体の移動に関する情報である
    位置推定装置。
  4. 　請求項１から３のいずれか一項に記載の位置推定装置において、
    　前記センサは、光を出射し、前記対象物で反射された反射光を受光することで前記対象物までの距離を測定する、
    位置推定装置。
  5. 　請求項４に記載の位置推定装置において、
    　前記第１取得部が取得した前記点群データに含まれる各データ点には、前記センサで受光した前記反射光の強度が関連付けられており、
    　前記第１推定部は、前記重畳点群データにおける複数のデータ点の強度を用いて前記対象物の位置を推定する
    位置推定装置。
  6. 　請求項５に記載の位置推定装置において、
    　前記点群データまたは前記重畳点群データに含まれる各データ点の強度を、そのデータ点の距離を用いて補正する強度補正部をさらに備え、
    　前記第１推定部は、補正後の強度を用いて前記対象物の位置を推定する
    位置推定装置。
  7. 　請求項１から６のいずれか一項に記載の位置推定装置において、
    　前記対象物は路面上の線である
    位置推定装置。
  8. 　請求項７に記載の位置推定装置において、
    　前記第１推定部は、路面上の線の端点位置を推定する
    位置推定装置。
  9. 　請求項８に記載の位置推定装置において、
    　前記第１推定部は、前記端点位置が前記重畳点群データ中の所定の領域内にある場合に、その重畳点群データを用いて、前記端点位置を推定する
    位置推定装置。
  10. 　請求項１から９のいずれか一項に記載の位置推定装置において、
    　前記重畳部は、前記移動体の速度に応じた回数の点群データが重ね合わされた、前記重畳点群データを生成する
    位置推定装置。
  11. 　請求項１から１０のいずれか一項に記載の位置推定装置において、
    　前記第１推定部による前記対象物の位置の推定結果を用いて、前記移動体の位置を推定する第２推定部をさらに備える
    位置推定装置。
  12. 　コンピュータにより実行される位置推定方法であって、
    　移動体に搭載されたセンサで得られた、複数タイミングの点群データを取得する第１取得ステップと、
    　前記移動体の移動情報を取得する第２取得ステップと、
    　前記複数タイミングの点群データが、前記移動情報に基づいて重ね合わされた、重畳点群データを生成する重畳ステップと、
    　前記重畳点群データを用いて対象物の位置を推定する第１推定ステップとを含む、
    位置推定方法。
  13. 　請求項１２に記載の位置推定方法をコンピュータに実行させるプログラム。
PCT/JP2023/008792 2022-03-14 2023-03-08 位置推定装置、位置推定方法、およびプログラム WO2023176617A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022039388 2022-03-14
JP2022-039388 2022-03-14

Publications (1)

Publication Number Publication Date
WO2023176617A1 true WO2023176617A1 (ja) 2023-09-21

Family

ID=88023124

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/008792 WO2023176617A1 (ja) 2022-03-14 2023-03-08 位置推定装置、位置推定方法、およびプログラム

Country Status (1)

Country Link
WO (1) WO2023176617A1 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018055222A (ja) * 2016-09-27 2018-04-05 日産自動車株式会社 走路検出方法及び走路検出装置
JP2020046427A (ja) * 2018-09-19 2020-03-26 バイドゥ オンライン ネットワーク テクノロジー （ベイジン） カンパニー リミテッド マルチセンサーのキャリブレーション方法とその装置、コンピュータ機器、媒体及び車両
JP2020165985A (ja) * 2020-06-16 2020-10-08 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体
JP2020170293A (ja) * 2019-04-02 2020-10-15 株式会社Ｉｈｉエアロスペース 画像表示方法及び遠隔操縦システム

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018055222A (ja) * 2016-09-27 2018-04-05 日産自動車株式会社 走路検出方法及び走路検出装置
JP2020046427A (ja) * 2018-09-19 2020-03-26 バイドゥ オンライン ネットワーク テクノロジー （ベイジン） カンパニー リミテッド マルチセンサーのキャリブレーション方法とその装置、コンピュータ機器、媒体及び車両
JP2020170293A (ja) * 2019-04-02 2020-10-15 株式会社Ｉｈｉエアロスペース 画像表示方法及び遠隔操縦システム
JP2020165985A (ja) * 2020-06-16 2020-10-08 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体

Similar Documents

Publication Publication Date Title
US11250288B2 (en) Information processing apparatus and information processing method using correlation between attributes
KR102441075B1 (ko) 노면표시기반 차량의 위치추정 방법 및 장치
JP6806891B2 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP6520740B2 (ja) 物体検出方法、物体検出装置、およびプログラム
EP3324359B1 (en) Image processing device and image processing method
JP7487388B2 (ja) 測定装置、測定方法、及び、プログラム
US11828603B2 (en) Measurement device, measurement method and program
JP7155284B2 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
CN108844538B (zh) 一种基于视觉/惯导的无人机避障航点生成方法
EP2052208A2 (en) Determining the location of a vehicle on a map
EP3260878B1 (en) Moving object detection device, program, and recording medium
EP1939585B1 (en) Object recognizing device
EP3258443B1 (en) Information processing device and method
JP2017211307A (ja) 測定装置、測定方法およびプログラム
WO2023176617A1 (ja) 位置推定装置、位置推定方法、およびプログラム
US10916034B2 (en) Host vehicle position estimation device
JP2023126399A (ja) 測定装置、測定方法およびプログラム
CN112712563A (zh) 相机取向估计
JP2023118759A (ja) 測定装置、測定方法およびプログラム
JP2020046411A (ja) データ構造、記憶装置、端末装置、サーバ装置、制御方法、プログラム及び記憶媒体
WO2018212290A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP2020046413A (ja) データ構造、記憶装置、端末装置、サーバ装置、制御方法、プログラム及び記憶媒体
JP7310587B2 (ja) 測距装置、測距方法、および測距プログラム
WO2022270183A1 (ja) 演算装置、速度算出方法
JP7117408B1 (ja) 位置算出装置、プログラム及び位置算出方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23770573

Country of ref document: EP

Kind code of ref document: A1