Position estimation device

Abstract

To accurately detect a moving object while reducing a processing load.SOLUTION: A position estimation device 50 comprises: a LiDAR 5 that is mounted on an own vehicle and detects an external environment condition around the own vehicle; a data acquisition unit 111 that acquires point cloud data including position information of measurement points on a surface of an object from which reflection waves of the LiDAR 5 are obtained, and velocity information indicating relative movement velocities of the measurement points; an estimation unit 112 that extracts road-surface point cloud data corresponding to a road surface from the point cloud data acquired by the data acquisition unit 111; a scan matching unit 115 that calculates, on the basis of the position information and the velocity information of measurement points corresponding to the road-surface point cloud data, a translational motion component indicating a movement amount and a movement direction of a translational motion of a moving object, and a rotational motion component indicating a rotation amount and a rotation direction of a rotational motion of the moving object; and a self-position estimation unit 116 that updates position information indicating the position of the own vehicle on the basis of the translational motion component and the rotational motion component calculated by the scan matching unit 115.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a position estimation device that estimates the self-position of a moving object. [Background technology]

[0002] In recent years, there has been a demand for vehicle control systems that improve traffic safety and contribute to the development of sustainable transportation systems. One such device is one that calculates the self-position of a moving object based on the amount of displacement between frames of feature points extracted from images of the moving object's forward field of view (see, for example, Patent Document 1). The device described in Patent Document 1 extracts feature points of the same stationary object from two consecutive frames of images, and calculates the self-position of the moving object from the amount of displacement between frames of these feature points. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2002-48513 Summary of the Invention [Problem to be solved by the invention]

[0004] However, when the device described in Patent Document 1 estimates the self-position based on the displacement of the feature points of objects around the moving body, the accuracy of estimating the self-position decreases when the displacement of the feature points of the objects between frames cannot be obtained, such as when there are no distinctive objects around the moving body. [Means for solving the problem]

[0005] A position estimation device according to one aspect of the present invention. [Effects of the Invention]

[0006] According to the present invention, the self-position of a moving object can be estimated with high accuracy regardless of the surrounding environment of the moving object. [Brief explanation of the drawings]

[0007] [Figure 1] 1 is a block diagram showing the configuration of a main part of a vehicle control device including a position estimation device according to an embodiment of the present invention; [Figure 2] 1 is a diagram for explaining the relationship between the relative movement speed of a moving object measured by a lidar and the movement direction of the moving object. FIG. [Figure 3A] FIG. 4 is a diagram for explaining estimation of a rotation angle of the host vehicle. [Figure 3B] FIG. 10 is a diagram showing an example of a rotational motion component estimated from a translational motion component. [Figure 4] FIG. 1 is a diagram schematically showing an example of point cloud data acquired by a lidar. [Figure 5A] FIG. 10 is a diagram schematically illustrating an example of still point cloud data of a previous frame. [Figure 5B] FIG. 10 is a diagram schematically showing an example of still point cloud data of a current frame. [Figure 6A] FIG. 10 is a diagram schematically showing how still point cloud data of a previous frame and still point cloud data of a current frame are aligned. [Figure 6B] FIG. 10 is a diagram showing still point cloud data of a previous frame and still point cloud data of a current frame after alignment. DETAILED DESCRIPTION OF THE INVENTION

[0008] Hereinafter, embodiments of the present invention will be described with reference to the drawings. A position estimation device according to an embodiment of the present invention can be applied to a vehicle having an automatic driving function, i.e., an automatic driving vehicle. Note that a vehicle to which a position estimation device according to this embodiment is applied may be referred to as a host vehicle to distinguish it from other vehicles. The host vehicle may be an engine vehicle having an internal combustion engine (engine) as a driving source, an electric vehicle having a driving motor as a driving source, or a hybrid vehicle having an engine and a driving motor as driving sources. The host vehicle can travel not only in an automatic driving mode in which no driving operation by the driver is required, but also in a manual driving mode in which the driver operates the vehicle.

[0009] When a self-driving vehicle is driving in self-driving mode (hereinafter referred to as self-driving or autonomous driving), it recognizes the external environment around the vehicle based on detection data from on-board detectors such as cameras and LiDAR (Light Detection and Ranging).Based on the recognition results, the self-driving vehicle generates a driving trajectory (target trajectory) for a predetermined time from the current time, and controls driving actuators so that the vehicle drives along the target trajectory.

[0010] 1 is a block diagram showing the configuration of a main part of a vehicle control device 100 including a position estimation device. The vehicle control device 100 has a controller 10, a communication unit 1, a positioning unit 2, an internal sensor group 3, a camera 4, a LIDAR 5, and a driving actuator AC. The vehicle control device 100 also has a position estimation device 50 that forms part of the vehicle control device 100. The position estimation device 50 detects objects around the vehicle based on detection data from the LIDAR 5.

[0011] The communication unit 1 communicates with various servers (not shown) via networks including wireless communication networks such as the Internet and mobile phone networks, and acquires map information, driving history information, traffic information, and the like from the servers periodically or at any timing. Networks include not only public wireless communication networks but also closed communication networks established for each specific management area, such as wireless LAN, Wi-Fi (registered trademark), and Bluetooth (registered trademark). The acquired map information is output to the memory unit 12, where it is updated. The positioning unit (GNSS unit) 2 has a positioning sensor that receives positioning signals transmitted from positioning satellites. The positioning satellites are artificial satellites such as GPS satellites and quasi-zenith satellites. The positioning unit 2 measures the current position (latitude, longitude, altitude) of the vehicle using the positioning information received by the positioning sensor.

[0012] The internal sensor group 3 is a collective term for a plurality of sensors (internal sensors) that detect the driving state of the host vehicle. For example, the internal sensor group 3 includes a vehicle speed sensor that detects the vehicle speed of the host vehicle, an acceleration sensor that detects the longitudinal acceleration and the lateral acceleration (lateral acceleration) of the host vehicle, a rotation speed sensor that detects the rotation speed of the driving source, a yaw rate sensor that detects the rotation angular velocity around the vertical axis of the center of gravity of the host vehicle, etc. The internal sensor group 3 also includes sensors that detect the driving operations of the driver in manual driving mode, such as operation of the accelerator pedal, operation of the brake pedal, operation of the steering wheel, etc.

[0013] The camera 4 has an imaging element such as a CCD or CMOS and captures images of the surroundings (front, rear, and sides) of the vehicle. The LIDAR 5 irradiates electromagnetic waves (reflected waves) into the three-dimensional space around the vehicle and detects the external environment around the vehicle based on the reflected waves. More specifically, the electromagnetic waves (laser light, etc.) irradiated by the LIDAR 5 are reflected at a point (measurement point) on the surface of an object, and the distance from the laser source to that point, the intensity of the reflected electromagnetic waves, the relative speed of the object located at that measurement point, etc. are measured. The LIDAR 5, which is attached to a predetermined position (front) of the vehicle, irradiates electromagnetic waves that scan the surroundings (front) of the vehicle in horizontal and vertical directions, detecting the position, shape, relative moving speed, etc. of objects ahead of the vehicle (moving objects such as other vehicles and stationary objects such as the road surface and structures).

[0014] Actuators AC are driving actuators for controlling the driving of the host vehicle. When the driving source is an engine, actuators AC include a throttle actuator that adjusts the opening of the engine's throttle valve (throttle opening). When the driving source is a driving motor, actuators AC include the driving motor. Actuators AC also include a brake actuator that operates the host vehicle's braking device and a steering actuator that drives the steering device.

[0015] The controller 10 is composed of an electronic control unit (ECU). More specifically, the controller 10 includes a computer having an arithmetic unit 11 such as a CPU (microprocessor), a storage unit 12 such as a ROM and a RAM, and other peripheral circuits (not shown) such as an I / O interface. Note that although multiple ECUs with different functions, such as an engine control ECU, a traction motor control ECU, and a braking device ECU, can be provided separately, for convenience, the controller 10 is shown in FIG. 1 as a collection of these ECUs.

[0016] The memory unit 12 stores highly accurate, detailed map information (referred to as high-accuracy map information). The high-accuracy map information includes road position information, road shape (curvature, etc.) information, road gradient information, intersection and branch point position information, number of lanes (driving lanes), lane width and position information for each lane (information on lane center positions and lane boundary lines), position information of landmarks (traffic lights, signs, buildings, etc.) on the map, and road surface profile information such as road surface irregularities. The memory unit 12 also stores various control programs, information such as thresholds used in the programs, and setting information for on-board detectors such as the LIDAR 5.

[0017] The calculation unit 11 has, as functional components, a data acquisition unit 111, an estimation unit 112, a speed calculation unit 113, a classification unit 114, a scan matching unit 115, a self-position estimation unit 116, and a driving control unit 117. As shown in Fig. 1 , the data acquisition unit 111, the estimation unit 112, the speed calculation unit 113, the classification unit 114, the scan matching unit 115, and the self-position estimation unit 116 are included in a position estimation device 50. Details of the data acquisition unit 111, the estimation unit 112, the speed calculation unit 113, the classification unit 114, the scan matching unit 115, and the self-position estimation unit 116 included in the position estimation device 50 will be described later.

[0018] In the autonomous driving mode, the driving control unit 117 generates a target trajectory based on the external environment around the vehicle (such as the size and position of an object and the relative movement speed of an object) detected by an on-board detector such as the LIDAR 5. Specifically, the driving control unit 117 generates a target trajectory based on the external environment around the vehicle detected by the on-board detector such as the LIDAR 5 and the position of the host vehicle estimated by the position estimation device 50, so as to avoid collision or contact with the object or to follow the object. The driving control unit 117 controls the actuators AC so that the host vehicle travels along the target trajectory. Specifically, the actuators AC are controlled along the target trajectory to adjust the accelerator opening and drive the braking device and the steering device. In the manual driving mode, the driving control unit 117 controls the actuators AC in accordance with a driving command (such as a steering operation) from the driver acquired by the internal sensor group 3.

[0019] The position estimation device 50 will be described in detail. As described above, the position estimation device 50 includes the data acquisition unit 111, the estimation unit 112, the velocity calculation unit 113, the classification unit 114, the scan matching unit 115, and the self-position estimation unit 116. The position estimation device 50 further includes a LIDAR 5.

[0020] First, the overall flow of the self-position estimation process executed by the position estimation device 50 will be described. FIG. 2 is a flowchart showing an example of the process executed by the CPU of the controller 10 of FIG. 1 in accordance with a predetermined program. The process shown in this flowchart is repeated at a predetermined cycle while the position estimation device 50 is running. More specifically, the process is repeated every time the data acquisition unit 111 acquires detection data of the LIDAR 5, that is, at predetermined time intervals (at time intervals determined by the frame rate of the LIDAR 5). Note that the intervals at which the process of FIG. 2 is executed may be variable rather than constant, taking into consideration the necessity for traffic safety and the calculation load, etc.

[0021] When detection data (point cloud data) from the lidar 5 is acquired (step S1), first, a process of classifying the point cloud data into moving point cloud data and stationary point cloud data (step S2) is executed.

[0022] Next, after offsetting the still point cloud data of the previous frame (step S4), a predetermined scan matching process is performed to overlay the still point cloud data of the previous frame on the still point cloud data of the current frame, thereby estimating the azimuth angle difference and movement vector of the host vehicle 101 (step S5). The movement vector represents the movement direction of a representative point (such as the center of gravity) of the host vehicle 101 between frames and the movement speed in that movement direction. The azimuth angle difference is the angular difference between the orientation of the host vehicle 101 in the current frame and the orientation in the previous frame. Hereinafter, the axis along the vehicle length direction of the moving object (the front-to-rear direction relative to the vehicle body) is defined as the X-axis, and the axes in the lateral and vertical directions relative to the vehicle length direction are defined as the Y-axis and Z-axis, respectively. The movement vector may be two-dimensional (X, Y) or three-dimensional (X, Y, Z). Furthermore, the azimuth angle difference may be a uniaxial angle (Z-axis rotation angle), a 2-axial angle (X-axis rotation angle, Z-axis rotation angle), or a 3-axial angle (X-axis rotation angle, Y-axis rotation angle, Z-axis rotation angle). The scan matching process may use ICP (Iterative Closest Point) or NDT (Normal Distributions Transform), or other methods. The azimuth angle difference and movement vector of the host vehicle 101 estimated in step S5 are accumulated, and a self-position estimation process (not shown) is executed in step S6 to estimate the host vehicle's position (the vehicle's traveling position) based on the accumulated azimuth angle difference and movement vector.

[0023] One example of an offset method in step S4 is a method of offsetting the still point cloud data of the previous frame using the movement vector and azimuth angle difference of the host vehicle 101 between frames (between the frame before last and the previous frame) estimated in the scan matching process (S5) of the previous cycle. On the other hand, in scenes where it is difficult to obtain feature points of stationary objects, corresponding feature points do not exist between frames or are difficult to find, so errors are likely to be included in the azimuth angle difference and movement vector of the host vehicle 101 estimated in the scan matching process (S5). Scenes where it is difficult to obtain feature points of stationary objects include, for example, a scene in which the host vehicle 101 is traveling on a road with almost no three-dimensional objects such as utility poles or trees around (such as a farm road running through fields), a scene in which the host vehicle 101 is traveling inside a tunnel, a scene in which the host vehicle is traveling on a road surrounded on both sides by large buildings or walls, and a scene in which the host vehicle is sandwiched between large vehicles such as buses and traveling alongside them.

[0024] If the azimuth angle difference and movement vector of the vehicle 101 estimated in the scan matching process (S5) contain errors, the errors may accumulate in the self-position estimation process (S6), potentially reducing the accuracy of the self-position estimation. To address this issue, the position estimation device 50 executes a process (steps S31 to S33) of estimating the movement speed and rotation angle of the vehicle 101 in parallel with the point cloud classification process (S2), as shown in Fig. 2. Then, the estimated movement speed and rotation angle of the vehicle 101 are used to execute an offset process (S4).

[0025] The processing in each step of FIG. 2 will be described in detail below. <Point Cloud Acquisition (S1)>

[0026] The data acquisition unit 111 acquires four-dimensional data (hereinafter referred to as point cloud data) including position information indicating three-dimensional position coordinates of measurement points on the surface of an object from which reflected waves of the LIDAR 5 are obtained, and velocity information indicating the relative movement velocity of the measurement points, as detection data of the LIDAR 5. The point cloud data is acquired by the LIDAR 5 in frame units, specifically at predetermined time intervals (at time intervals determined by the frame rate of the LIDAR 5). <Point Cloud Classification (S2)>

[0027] The classification unit 114 classifies the point cloud data acquired by the data acquisition unit 111 into moving point cloud data corresponding to measurement points whose absolute values ​​of absolute movement speeds are equal to or greater than a predetermined speed Th_V, and stationary point cloud data corresponding to measurement points whose absolute values ​​are less than the predetermined speed Th_V. The absolute movement speeds of the measurement points are calculated by the speed calculation unit 113 in the processing of step S32, as will be described later.

[0028] <Extraction of road surface points (S31)>

[0029] The estimation unit 112 extracts point cloud data from the point cloud data acquired by the data acquisition unit 111, excluding information on measurement points corresponding to three-dimensional objects, that is, point cloud data corresponding to the road surface around the vehicle 101 (hereinafter referred to as road surface point cloud data). Note that the road surface point cloud data may be extracted by a plane approximation method or by other methods. <Estimation of Vehicle's Moving Speed ​​and Direction (S32)>

[0030] The estimation unit 112 calculates a unit vector e i indicating the direction of the relative movement speed v i based on the road surface point cloud data, i.e., the position coordinates (xi, yi, zi) included in the four-dimensional data (xi, yi, zi, vi) of the measurement points Pi (i=1, 2, ..., n) corresponding to the road surface. Specifically, the estimation unit 112 calculates the unit vector e i by the following equation (i):

[0031]

number

[0032] The estimation unit 112 estimates the moving speed (absolute moving speed) Vself of the host vehicle 101. Specifically, the estimation unit 112 sets a conversion equation for converting the relative moving speed vi of the measurement points Pi corresponding to the road surface into the absolute moving speed as the objective function L, and solves an optimization problem to optimize the objective function L so as to approach zero. Because the measurement points Pi are measurement points on the road surface, the absolute speed of each of these measurement points should be zero. Therefore, by optimizing the objective function L so as to approach zero, it is possible to estimate the correct Vself. Vself is expressed by velocity components in the X, Y, and Z axes as shown in the following equation (ii). The objective function L is expressed by the following equation (iii). By solving the optimization problem, Vself that makes the right-hand side of equation (iii) zero is searched for. Note that Vself may be set to zero as an initial value, or Vself estimated in the previous frame may be set.

[0033]

number

number

[0034] In equation (iii), A is a matrix of unit vectors ei of n measurement points corresponding to the road surface, and is expressed by equation (iv). Also in equation (iii), V is a 1×n matrix representing the velocity components (relative movement velocity) of n measurement points Pi corresponding to the road surface, and is expressed by equation (v). The estimation unit 112 acquires Vself obtained by solving the optimization problem as an estimate of the absolute movement velocity of the host vehicle 101 in the current frame. The estimated value Vself acquired by the estimation unit 112 is used in estimating the rotation angle of the host vehicle (S33).

[0035]

number

number

[0036] The speed calculation unit 113 calculates the absolute movement speeds of all measurement points, more specifically, all measurement points including measurement points corresponding to three-dimensional objects, based on the absolute movement speed Vself of the host vehicle 101 estimated by the estimation unit 112. This absolute movement speed has a negative value when approaching the host vehicle and a positive value when moving away. As described above, the absolute movement speed calculated by the speed calculation unit 113 is used in classifying the point cloud (S2). <Estimation of Vehicle Rotation Angle (S33)>

[0037] 3A is a diagram for explaining estimation of the rotation angle of the host vehicle. The movement of an object in three-dimensional space can be expressed by six degrees of freedom: translational motion components, specifically, movement velocities vx, vy, vz in the X-axis, Y-axis, and Z-axis directions, and rotational motion components, specifically, rotation angles X_angle, Y_angle, and Z_angle in the X-axis, Y-axis, and Z-axis directions. On the other hand, the movement of the host vehicle 101 can be expressed by three degrees of freedom: the movement speed (vehicle speed) of the host vehicle 101, the tire angle, and the installation position of the rider. Because the host vehicle 101 is one of the objects, it is assumed that there is a causal relationship between the three degrees of freedom expressing the movement of the host vehicle 101 and the six degrees of freedom expressing the movement of the object in three-dimensional space (the translational motion components and the rotational motion components). Furthermore, because both the translational motion component and the rotational motion component have a causal relationship with the three degrees of freedom that represent the movement of the host vehicle 101, it is assumed that there is a correlation between the translational motion component and the rotational motion component, as shown in FIG. 3A. Therefore, it should be possible to estimate the rotational motion component of the host vehicle 101 from the translational motion component Vself of the host vehicle 101 calculated based on road surface point cloud data, utilizing the causal relationship and correlation. FIG. 3B is a diagram showing an example of the Z-axis rotational motion component Z_angle estimated from the vx and vy of the translational motion component Vself using multiple regression analysis. The vertical axis of the graph in FIG. 3B represents the estimated value (predicted value) of Z_angle, and the horizontal axis represents the actual measured value of Z_angle. As shown in FIG. 3B, the estimated value of the Z-axis rotational motion component Z_angle using multiple regression analysis is approximately equal to the actual measured value, indicating that the Z-axis rotational motion component Z_angle can be estimated from the vx and vy of the translational motion component Vself. 3B only shows the estimated result of the rotational motion component Z_angle about the Z axis of the host vehicle 101, but the rotational motion component Y_angle about the Y axis of the host vehicle 101 can also be estimated from the vx and vz of the translational motion component Vself of the host vehicle 101. The rotational motion component X_angle about the X axis of the host vehicle 101 can also be estimated from the vy and vz of the translational motion component Vself of the host vehicle 101. The method for estimating the rotational motion component of the host vehicle 101 is not limited to multiple regression analysis, and other methods such as machine learning may also be used.Alternatively, a motion model used at the time of vehicle design may be used to estimate the rotational motion component of the host vehicle 101. Furthermore, the rotational motion component of the host vehicle 101 may be directly acquired using a sensor such as a gyro sensor. <Offset processing (S4), scan matching processing (S5)>

[0038] First, the scan matching process (S5) will be described. FIGS. 4, 5A, 5B, 6A, and 6B are diagrams for explaining the scan matching process between the previous frame and the current frame. In FIG. 4, the arrow attached to the host vehicle 101 represents the vehicle length direction (the front-to-rear direction of the vehicle body) of the host vehicle 101, i.e., the X-axis direction. The lateral direction (the left-right direction in FIG. 4) and the height direction (the direction from back to front in FIG. 4) relative to the X-axis direction represent the Y-axis and Z-axis directions. The definitions of the X-axis, Y-axis, and Z-axis directions are similar in other figures, so the X-axis, Y-axis, and Z-axis are omitted in those figures. FIG. 4 shows an example of a schematic diagram of point cloud data acquired by the LIDAR 5 at a past point in time (time t1) viewed from above (in the Z-axis direction). Regions N1 to N6 schematically represent measurement point clouds corresponding to stationary objects, more specifically, the positions and sizes of the measurement point clouds. Stationary objects include the road surface on which the vehicle 101 is traveling, structures such as walls and median strips installed on the side of the road, and other vehicles parked on the shoulder of the road. Areas M31 and M32 schematically represent measurement point clouds corresponding to moving objects, more specifically, the positions and sizes of the measurement point clouds. Note that, hereinafter, objects detected by the lidar 5 include people as well. Therefore, moving objects include not only vehicles such as moving cars and bicycles, but also moving people (pedestrians, etc.). Arrows attached to areas M31 and M32 indicate the direction of movement of the moving objects.

[0039] Fig. 5A schematically shows the stationary point cloud data classified from the point cloud data of Fig. 4, i.e., point cloud data acquired by the LIDAR 5 at a past time point (time t1). Fig. 5B schematically shows the stationary point cloud data classified from the point cloud data acquired by the LIDAR 5 at the present time point (time t2) a predetermined time has elapsed since a predetermined time point in the past (time t1).

[0040] The scan matching unit 115 first performs a predetermined scan matching process to align the still point cloud data of the previous frame ( FIG. 5A ) with the still point cloud data of the current frame ( FIG. 5B ) and estimate the azimuth angle difference and movement vector of the vehicle 101 over a predetermined time (between frames). More specifically, the scan matching process aligns the still point cloud data of the previous frame and the current frame other than the road surface point cloud data, i.e., the still point cloud data corresponding to three-dimensional objects (hereinafter referred to as still three-dimensional point cloud data). FIGS. 6A and 6B schematically illustrate how the still point cloud data of the previous frame ( FIG. 5A ) and the still point cloud data of the current frame ( FIG. 5B ) are aligned. In FIG. 6A , the still point cloud data of the previous frame is indicated by a dashed line, and the still point cloud data of the current frame is indicated by a solid line. The scan matching unit 115 searches for (estimates) the azimuth angle difference and movement vector of the host vehicle 101 so that the measurement point groups N1 to N6 of the previous frame, indicated by dashed lines, overlap (match) the measurement point groups N1 to N6 of the current frame, indicated by solid lines. FIG. 6B shows the still point cloud data of the previous frame after alignment and the still point cloud data of the current frame. In FIG. 6B, the angle MA represents the azimuth angle difference of the host vehicle 101 between frames. The white arrow MV represents the movement vector of the host vehicle 101 between frames. The white circle in the figure schematically represents the center of gravity of the host vehicle 101. The scan matching unit 115 solves an optimization problem that minimizes the deviation (error) between the positions of the measurement point groups N1 to N6 of the previous frame after alignment and the positions of the measurement point groups N1 to N6 of the current frame, and outputs the final search results (estimation results) of the azimuth angle difference and movement vector of the host vehicle 101. In order to reduce the calculation load, the three-dimensional point cloud data may be converted into two-dimensional point cloud data represented in an XY coordinate system before the above alignment is performed.

[0041] In the offset process (S4), the scan matching unit 115 offsets the still point cloud data of the previous frame using the movement vector and azimuth angle difference of the host vehicle 101 between frames (between the frame before last and the previous frame) estimated in the scan matching process (S5) of the previous cycle. In the scan matching process (S5) executed after the offset process (S4), the scan matching unit 115 uses the offset still point cloud data of the previous frame as initial values ​​and aligns it with the still point cloud data of the current frame. In this way, the movement vector and azimuth angle of the host vehicle 101 estimated in the scan matching process are used as initial values ​​when solving the optimization problem in the next scan matching process. <Self-position estimation processing (S6)>

[0042] The self-position estimation unit 116 accumulates the azimuth angle difference and movement vector of the vehicle 101 estimated in the scan matching process (S5), and estimates the self-position (the traveling position of the vehicle 101) based on the accumulated azimuth angle difference and movement vector.

[0043] According to the embodiment described above, the following advantageous effects are achieved. (1) The position estimation device 50 is mounted on the vehicle 101 and includes a lidar 5 that irradiates electromagnetic waves around the vehicle 101 and detects the external environment around the moving body based on the reflected waves; a data acquisition unit 111 that acquires point cloud data at predetermined time intervals that indicates the detection results of the lidar 5, including position information of measurement points on the surface of an object from which the reflected waves are obtained and speed information that indicates the relative moving speed of the measurement points; an estimation unit 112 that serves as an extraction unit that extracts road surface point cloud data corresponding to the road surface from the point cloud data acquired by the data acquisition unit 111; and a position estimation unit 113 that estimates the position of the measurement points corresponding to the road surface point cloud data. The vehicle 101 includes a scan matching unit 115 as a motion component calculation unit that calculates translational motion components (vx, vy, vz in FIG. 3A) indicating the amount of movement and direction of translational motion of the vehicle 101 and rotational motion components (X_angle, Y_angle, Z_angle in FIG. 3A) indicating the amount of rotation of rotational motion of the vehicle 101 based on position information and speed information, and a self-position estimation unit 116 as an update unit that updates position information indicating the position of the vehicle 101 based on the translational motion components and rotational motion components calculated by the scan matching unit 115. This makes it possible to accurately estimate the position of the vehicle 101 regardless of the environment around the vehicle 101 while it is traveling.

[0044] (2) The position estimation device 50 further includes a velocity calculation unit 113 that calculates the absolute movement velocity of each of the multiple measurement points corresponding to the point cloud data acquired by the data acquisition unit 111 based on the velocity information of the measurement points and the translational motion component of the host vehicle 101, and a classification unit 114 that classifies the point cloud data into static point cloud data for which the absolute value of the absolute movement velocity calculated by the velocity calculation unit 113 is less than a predetermined velocity and moving point cloud data for which the absolute value is equal to or greater than the predetermined velocity. The host position estimation unit 116 further offsets static point cloud data from a time point a predetermined time before the current time based on the translational motion component and rotational motion component calculated by the scan matching unit 115, searches for translational motion component and rotational motion component that minimize the position error between corresponding measurement points in the current still point cloud data and the offset still point cloud data, and updates the position information of the host vehicle 101 based on the translational motion component and rotational motion component obtained by the search. This allows appropriate setting of initial values ​​when solving an optimization problem in the scan matching process, and prevents erroneous local solutions from being obtained as the search result.

[0045] (3) The scan matching unit 115 calculates the translational motion component of the host vehicle 101 based on the position information and speed information of the measurement points corresponding to the road surface point cloud data, and calculates the rotational motion component from the calculated translational motion component based on a predetermined correlation between the translational motion component and the rotational motion component. The correlation is predetermined based on the installation position of the rider 5 on the host vehicle 101, the traveling speed of the host vehicle 101, and the steering angle (tire angle) of the host vehicle 101. In this way, by acquiring information used in the offset process (S4) (the traveling speed and rotation angle of the host vehicle 101) from the current frame, the host vehicle's position can be estimated with high accuracy even in scenes where corresponding feature points between frames are difficult to find or do not exist.

[0046] The above-described embodiment can be modified into various forms. Modifications will be described below. In the above-described embodiment, a LIDAR 5 as a detector is mounted on a vehicle, and irradiates electromagnetic waves into a three-dimensional space around the vehicle and detects the external environment around the vehicle based on the reflected waves. However, the detector may be a device other than a LIDAR, such as a radar. Furthermore, the moving body on which the detector is mounted may be a device other than a vehicle, such as a self-propelled robot.

[0047] In the above embodiment, the estimation unit 112 as an information acquisition unit performs alignment between the still point cloud data of the previous frame ( FIG. 5A ) and the still point cloud data of the current frame ( FIG. 5B ) to estimate the azimuth angle difference and movement vector of the host vehicle 101 at a predetermined time (between frames). However, the information acquisition unit may perform the alignment after converting the three-dimensional still point cloud data into two-dimensional still point cloud data represented in an XY coordinate system. Furthermore, the information acquisition unit may perform the alignment using not only the still point cloud data of a past time point (time t1) but also still point cloud data of multiple past time points, i.e., not only the still point cloud data of the previous frame but also still point cloud data of multiple past frames. In this way, by using the still point cloud data of multiple past frames, the alignment can be performed well even when a stationary object ahead of the host vehicle 101 is temporarily occluded by another vehicle or the like, thereby improving robustness.

[0048] Furthermore, in the above embodiment, the position estimation device 50 is applied to an autonomous vehicle, but the position estimation device 50 can also be applied to vehicles other than autonomous vehicles. For example, the position estimation device 50 can also be applied to a manually driven vehicle equipped with an ADAS (Advanced Driver-Assistance Systems).

[0049] The above description is merely an example, and the present invention is not limited to the above-described embodiment and modifications, as long as the features of the present invention are not impaired. One or more of the above-described embodiment and modifications can be arbitrarily combined, and modifications can also be combined with each other. [Explanation of symbols]

[0050] 1 communication unit, 2 positioning unit, 3 internal sensor group, 4 camera, 5 lidar, 10 controller, 11 calculation unit, 12 memory unit, 111 data acquisition unit, 112 estimation unit, 113 speed calculation unit, 114 classification unit, 115 scan matching unit, 116 self-position estimation unit, 117 driving control unit, AC actuator

Claims

1. a detector mounted on the moving body, which irradiates electromagnetic waves around the moving body and detects an external environment around the moving body based on reflected waves; a data acquisition unit that acquires point cloud data at predetermined time intervals that indicates the detection results of the detector, the point cloud data including position information of measurement points on the surface of the object from which the reflected waves are obtained and velocity information that indicates the relative movement velocity of the measurement points; an extraction unit that extracts road surface point cloud data corresponding to a road surface from the point cloud data acquired by the data acquisition unit; a motion component calculation unit that calculates translational motion components that indicate the amount of movement and the direction of movement of the translational motion of the moving body and rotational motion components that indicate the amount of rotation and the direction of rotation of the rotational motion of the moving body based on the position information and the speed information of the measurement points corresponding to the road surface point cloud data; an updating unit that updates position information indicating a position of the moving body based on the translational motion component and the rotational motion component calculated by the motion component calculation unit.

2. 2. The position estimation device according to claim 1, a velocity calculation unit that calculates an absolute movement velocity of each of the plurality of measurement points corresponding to the point cloud data based on the velocity information of the measurement points and the translational motion component of the moving object; a classification unit that classifies the point cloud data into still point cloud data in which the absolute value of the absolute moving speed calculated by the speed calculation unit is less than a predetermined speed and moving point cloud data in which the absolute value is equal to or greater than the predetermined speed, the updating unit further offsets the still point cloud data at a past point in time that is a predetermined time before the current point in time based on the translational motion component and the rotational motion component calculated by the motion component calculation unit, searches for the translational motion component and the rotational motion component that minimize a position error between the corresponding measurement points of the still point cloud data at the current point in time and the still point cloud data after the offset, and updates the position information of the moving body based on the translational motion component and the rotational motion component obtained by the search.

3. 2. The position estimation device according to claim 1, The motion component calculation unit calculates the translational motion component of the moving body based on the position information and the velocity information of the measurement point corresponding to the road surface point cloud data, and calculates the rotational motion component from the calculated translational motion component based on a predetermined correlation between the translational motion component and the rotational motion component.

4. 4. The position estimation device according to claim 3, The position estimation device according to claim 1, wherein the correlation is determined in advance based on the installation position of the detector, the traveling speed of the moving body, and the steering angle of the moving body.

5. 5. The position estimation device according to claim 1, A position estimation device characterized in that the detector is a lidar.

Images