Position estimation device

Abstract

A position estimation device (50) is provided with: a detector (5) that is mounted on a host vehicle and detects the outside situation around the host vehicle; a data acquisition unit (111) that acquires point cloud data including position information of a measurement point on the surface of the object from which the reflected wave of the detector (5) is obtained, and velocity information indicating the relative movement velocity of the measurement point; an extraction unit (112) that extracts road surface point cloud data corresponding to the road surface from the point cloud data acquired by the data acquisition unit (111); a motion component calculation unit (115) that calculates, on the basis of position information and velocity information of a measurement point corresponding to the road surface point cloud data, a translational motion component indicating the amount and direction of movement of the translational motion of the moving body, and a rotational motion component indicating the amount and direction of rotation of the rotational motion of the moving body; and an update unit (116) that updates position information indicating the position of the host vehicle on the basis of the translational motion component and the rotational motion component calculated by the motion component calculation unit (115).

Description

Technical Field

[0001] The present invention relates to a position estimating device for estimating the own position of a mobile object. Background Art

[0002] In recent years, there has been a demand for a vehicle control system that improves traffic safety and contributes to the development of a sustainable transportation system. As such, a device that calculates a moving object's own position based on the inter-frame displacement of feature points extracted from images of the moving object's forward field of view is known (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 and calculates the moving object's own position based on the inter-frame displacement of these feature points.

[0003] However, as in the device described in Patent Document 1, if the self-position is estimated based on the displacement of the feature points of objects around the moving body, the accuracy of the self-position estimation is reduced when the displacement of the feature points of the objects cannot be obtained between frames, such as when there are no characteristic objects around the moving body.

[0004] Prior art literature

[0005] Patent Literature

[0006] Patent Document 1: Japanese Patent Application Publication No. 2002-48513 (JP 2002-048513 A). Summary of the Invention

[0007] A position estimation device according to one technical solution of the present invention comprises: a detector mounted on a mobile body, irradiating electromagnetic waves around the mobile body, and detecting external conditions around the mobile body based on the reflected waves; a data acquisition unit, which acquires point cloud data at each specified time representing a detection result of the detector, the detection result including position information of a measurement point on the surface of the object from which the reflected waves are obtained and speed information representing the relative movement speed of the measurement point; an extraction unit, which extracts road surface point cloud data corresponding to the road surface from the point cloud data acquired by the data acquisition unit; a motion component calculation unit, which calculates a translational motion component representing the amount and direction of translational motion of the mobile body and a rotational motion component representing the amount and direction of rotational motion of the mobile body based on the position information and speed information of the measurement point corresponding to the road surface point cloud data; and an updating unit, which updates the position information representing the position of the mobile body based on the translational motion component and the rotational motion component calculated by the motion component calculation unit. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] The objects, features and advantages of the present invention will be further clarified through the following description of the embodiments in conjunction with the accompanying drawings.

[0009] Figure 1This is a block diagram showing a main configuration of a vehicle control device including a position estimation device according to an embodiment of the present invention;

[0010] Figure 2 It is shown by Figure 1 A flowchart of an example of a process executed by the CPU of the controller 10;

[0011] Figure 3A is a diagram for explaining the estimation of the rotation angle of the host vehicle;

[0012] Figure 3B FIG. 1 is a diagram showing an example of a rotational motion component estimated from a translational motion component;

[0013] Figure 4 FIG is a diagram schematically showing an example of point cloud data acquired by a laser radar;

[0014] Figure 5A Schematically shows an example of stationary point cloud data of the previous frame;

[0015] Figure 5B is a diagram schematically showing an example of stationary point cloud data of the current frame;

[0016] Figure 6A Schematically illustrates how the static point cloud data of the previous frame and the static point cloud data of the current frame are aligned.

[0017] Figure 6B 3 is a diagram showing the aligned static point cloud data of the previous frame and the static point cloud data of the current frame. DETAILED DESCRIPTION

[0018] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. The position estimation device of the embodiment of the present invention can be applied to a vehicle having an automatic driving function, i.e., an automatic driving vehicle. It should be noted that sometimes, the vehicle to which the position estimation device of the present embodiment is applied is referred to as the present vehicle, in distinction from other vehicles. The present vehicle may be any one of an engine vehicle having an internal combustion engine (engine) as a driving source, an electric vehicle having a driving motor as a driving source, and a hybrid vehicle having an engine and a driving motor as driving sources. The present vehicle can travel not only in an automatic driving mode that does not require the driver's driving operation, but also in a manual driving mode in which the driver performs the driving operation.

[0019] When operating in autonomous driving mode (hereinafter referred to as automated driving or autonomous driving), an autonomous vehicle identifies the surrounding environment based on data from onboard sensors such as cameras and LiDAR (Light Detection and Ranging). Based on this identification, the autonomous vehicle generates a driving trajectory (target trajectory) extending from the current point in time to a predetermined time, and controls its driving actuators to ensure the vehicle follows the target trajectory.

[0020] Figure 1 This is a block diagram showing the main components of a vehicle control device 100, including a position estimation device. The vehicle control device 100 includes a controller 10, a communication unit 1, a positioning unit 2, an internal sensor group 3, a camera 4, a laser radar 5, and actuators AC for driving. Furthermore, the vehicle control device 100 includes a position estimation device 50, which constitutes a portion of the vehicle control device 100. The position estimation device 50 detects objects around the vehicle based on detection data from the laser radar 5.

[0021] The communication unit 1 communicates with various servers not shown in the figure through a network including a wireless communication network represented by the Internet, a mobile phone network, etc., and obtains map information, driving history information, traffic information, etc. from the server regularly or at any time. The network includes not only public wireless communication networks, but also closed communication networks set up in each specified management area, such as wireless LAN, Wi-Fi (registered trademark), Bluetooth (registered trademark), etc. The acquired map information is output to the storage unit 12, and the map information is updated. The positioning unit (GNSS unit) 2 has a positioning sensor that receives positioning signals sent from positioning satellites. Positioning satellites are artificial satellites such as GPS satellites and quasi-zenith satellites. The positioning unit 2 uses the positioning information received by the positioning sensor to measure the current position (latitude, longitude, altitude) of the vehicle.

[0022] The internal sensor group 3 is a collective term for multiple sensors (internal sensors) that detect the vehicle's driving state. For example, the internal sensor group 3 includes a speed sensor that detects the vehicle's speed, acceleration sensors that detect the vehicle's front-to-back acceleration and left-to-right acceleration (lateral acceleration), a rotational speed sensor that detects the rotational speed of the vehicle's driving source, and a yaw rate sensor that detects the angular velocity of the vehicle's center of gravity around its vertical axis. Sensors that detect driver operations in manual driving mode, such as those on the accelerator pedal, brake pedal, and steering wheel, are also included in the internal sensor group 3.

[0023] The camera 4 has imaging elements such as a CCD (charge-coupled device) and a CMOS (complementary metal oxide semiconductor) and captures the surroundings of the vehicle (in front, behind, and to the sides). The laser radar 5 radiates electromagnetic waves (reflected waves) into the three-dimensional space around the vehicle and detects the external conditions around the vehicle based on the reflected waves. More specifically, the electromagnetic waves (laser, etc.) emitted by the laser radar 5 are reflected and returned at a certain point (measurement point) on the surface of an object, thereby measuring the distance from the laser source to that point, the intensity of the reflected electromagnetic waves, the relative speed of the object at that measurement point, etc. The electromagnetic waves of the laser radar 5 installed at a specified position (front) of the vehicle scan the surroundings (in front) of the vehicle in the horizontal and vertical directions, thereby detecting the position, shape, relative movement speed, etc. of objects in front of the vehicle (moving objects such as other vehicles, stationary objects such as road surfaces and structures).

[0024] Actuators AC are driving actuators used to control the vehicle's travel. If the driving source is an engine, actuators AC include a throttle actuator that adjusts the engine's throttle valve opening (throttle opening). If the driving source is a travel motor, the travel motor is included in actuators AC. Actuators AC also include a brake actuator that activates the vehicle's brakes and a steering actuator that drives the steering system.

[0025] The controller 10 is composed of an electronic control unit (ECU). More specifically, the controller 10 includes a computer having a computing unit 11 such as a CPU (microprocessor), a storage unit 12 such as a ROM (read-only memory) and a RAM (random access memory), and other peripheral circuits not shown in the figure, such as an I / O (input / output) interface. It should be noted that multiple ECUs with different functions, such as an engine control ECU, a travel motor control ECU, and a brake device ECU, can be set separately, but for convenience, Figure 1 Controller 10 is shown in FIG as a collection of these ECUs.

[0026] The storage unit 12 stores high-precision, detailed map information (referred to as high-precision map information). This high-precision map information includes information on road locations, road shapes (such as curvature), road slopes, intersections, and forks, the number of lanes (driving lanes), lane widths, and the location of each lane (including lane center positions and lane boundary lines), the location of landmarks (such as traffic lights, signs, and buildings) that mark the map, and road surface contour information such as surface roughness. The storage unit 12 also stores various control programs, information such as thresholds used in these programs, and settings for on-board sensors such as the lidar 5.

[0027] The computing unit 11 includes 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 functional structures. Figure 1 As shown, 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 are included in the position estimation device 50. 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 included in the position estimation device 50 are described in detail below.

[0028] In the automatic driving mode, the driving control unit 117 generates a target trajectory based on the external conditions around the vehicle (the size and position of the object, the relative moving speed, etc.) detected by the vehicle-mounted detectors such as the laser radar 5. Specifically, the driving control unit 117 generates a target trajectory in a manner that avoids collision, contact with, or following the object based on the external conditions around the vehicle detected by the vehicle-mounted detectors such as the laser radar 5 and the position of the vehicle estimated by the position estimation device 50. The driving control unit 117 controls the actuator AC so that the vehicle travels along the target trajectory. Specifically, the actuator AC is controlled along the target trajectory to adjust the accelerator opening, drive the brake device, or drive the steering device. It should be noted that in the manual driving mode, the driving control unit 117 controls the actuator AC based on the driving instructions (steering operation, etc.) from the driver obtained by the internal sensor group 3.

[0029] 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 own position estimation unit 116. The position estimation device 50 also includes the laser radar 5.

[0030] First, the entire flow of the self-position estimation process executed by the position estimation device 50 will be described. Figure 2 is Figure 1 The flowchart is an example of a process executed by the CPU of the controller 10 according to a predetermined program. The process shown in the flowchart is repeated at a predetermined cycle during the startup of the position estimation device 50. More specifically, each time the detection data of the laser radar 5 is acquired by the data acquisition unit 111, it is repeated at a predetermined time (a time interval determined by the frame rate of the laser radar 5). It should be noted that the execution may be repeated in accordance with the necessity of traffic safety, calculation load, etc. Figure 2 The processing interval is variable rather than constant.

[0031] When the detection data (point cloud data) of the laser radar 5 is acquired (step S1 ), first, a process of classifying the point cloud data into moving point cloud data and stationary point cloud data is performed (step S2 ).

[0032] Next, after offsetting the static point cloud data of the previous frame (step S4), the prescribed scan matching processing is performed to estimate the azimuth angle difference and movement vector of the vehicle 101 by overlapping the static point cloud data of the previous frame with the static point cloud data of the current frame (step S5). The movement vector represents the moving direction of the representative point (center of gravity, etc.) of the vehicle 101 between frames and the moving speed in the moving direction. The azimuth angle difference is the angular difference between the orientation of the vehicle 101 in the current frame and the orientation in the previous frame. In the following, the axis along the vehicle length direction of the moving body (the front-to-back direction for the vehicle body) is referred to as the X-axis, and the axes in the horizontal and height directions relative to the vehicle length direction are referred to as the Y-axis and the Z-axis, respectively. It should be noted that the movement vector can be two-dimensional (X, Y) or three-dimensional (X, Y, Z). In addition, the azimuth angle difference can be the angle of one axis (Z-axis rotation angle), the angle of two axes (X-axis rotation angle, Z-axis rotation angle), or the angle of three axes (X-axis rotation angle, Y-axis rotation angle, Z-axis rotation angle). In the scan matching process, ICP (Iterative Closest Point), NDT (Normal Distributions Transform), or other methods can be used. The azimuth angle difference and movement vector of the host vehicle 101 estimated in step S5 are accumulated, and based on the accumulated azimuth angle difference and movement vector, in step S6, a self-position estimation process (not shown) is performed to estimate the own position (the driving position of the host vehicle 101).

[0033] As an example of an offset method in step S4, there is a method of offsetting the stationary point cloud data of the previous frame using the inter-frame motion vector and azimuth angle difference of the host vehicle 101 (between the previous frame and the previous frame) estimated in the scan matching process (S5) of the previous cycle. On the other hand, in scenarios where it is difficult to obtain feature points of stationary objects, corresponding feature points do not exist or are difficult to find between frames, so the azimuth angle difference and motion vector of the host vehicle 101 estimated in the scan matching process (S5) are likely to contain errors. Scenarios where it is difficult to obtain feature points of stationary objects include scenarios where the host vehicle 101 is traveling on a road with few surrounding objects such as utility poles and trees (such as a farm road through farmland), driving in a tunnel, driving on a road surrounded by large buildings and walls, and traveling between and parallel to large vehicles such as buses.

[0034] If the azimuth angle difference and movement vector of the vehicle 101 estimated in the scan matching process (S5) contain errors, the errors may be accumulated in the own position estimation process (S6), thereby reducing the estimation accuracy of the own position. Figure 2 As shown, the position estimation device 50 performs a process (steps S31 to S33) of estimating the moving speed and rotation angle of the host vehicle 101 in parallel with the point cloud classification process (S2). Then, the estimated moving speed and rotation angle of the host vehicle 101 are used to perform an offset process (S4).

[0035] Detailed description Figure 2 The processing in each step.

[0036] <Acquisition of Point Cloud (S1)>

[0037] The data acquisition unit 111 acquires four-dimensional data (hereinafter referred to as point cloud data) including position information indicating the three-dimensional position coordinates of a measurement point on the surface of an object that receives a reflected wave from the laser radar 5 and speed information indicating the relative movement speed of the measurement point, as detection data of the laser radar 5. The point cloud data is acquired by the laser radar 5 in units of frames, specifically, in units of predetermined time (a time interval determined by the frame rate of the laser radar 5).

[0038] <Point Cloud Classification (S2)>

[0039] 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 moving speed is greater than or equal to a predetermined speed Th V, and stationary point cloud data corresponding to measurement points whose absolute moving speed is less than the predetermined speed Th V. As will be described later, the absolute moving speed of the measurement point is calculated by the speed calculation unit 113 in the process of step S32.

[0040] <Extraction of Road Surface Point Cloud (S31)>

[0041] The estimation unit 112 extracts point cloud data other than the information on the measurement points corresponding to the three-dimensional objects from the point cloud data acquired by the data acquisition unit 111, namely, point cloud data corresponding to the road surface surrounding the vehicle 101 (hereinafter referred to as road surface point cloud data). It should be noted that the road surface point cloud data may be extracted using a planar approximation method or other methods.

[0042] <Estimation of the Vehicle's Moving Speed and Moving Direction (S32)>

[0043] The estimation unit 112 estimates the road surface point cloud data, that is, the four-dimensional data (xi, y i, zi, vi) contains the position coordinates (xi, y i, zi), and calculates the unit vector ei indicating the direction of the relative movement speed vi. Specifically, the estimating unit 112 calculates the unit vector ei using the following equation (i).

[0044]

[0045] The estimation unit 112 estimates the moving speed (absolute moving speed) Vself of the vehicle 101. Specifically, the estimation unit 112 sets the conversion formula for converting the relative moving speed vi of the measurement point Pi corresponding to the road surface into the absolute moving speed as the objective function L, and solves the optimization problem of optimizing the objective function L to be close to zero. The measurement point Pi is a measurement point on the road surface, so the absolute speed of each measurement point should be zero. Therefore, by optimizing the objective function L to be close to zero, the correct Vself can be estimated. Vself is shown in the following formula (ii), which is expressed by the speed components in the X, Y, and Z axis directions. The objective function L is expressed by the following formula (iii). By solving the above optimization problem, Vself that makes the right side of formula (iii) zero can be searched. It should be noted that Vself can be set to zero as the initial value, or it can be set to the Vself estimated in the previous frame.

[0046] V self =(v x , v y , v z ) (ii)

[0047] L(V, f(A, V self ))=V+A·V self T (iii)

[0048] In formula (iii), A is the matrix of the unit vectors ei of the n measurement points corresponding to the road surface, which is given by formula (iii). v ). Furthermore, in equation (iii), V is a 1×n matrix representing the velocity components (relative moving speeds) of n measurement points Pi corresponding to the road surface, as represented by equation (v). The estimating unit 112 obtains Vself, obtained by solving the aforementioned optimization problem, as an estimated value of the absolute moving speed of the host vehicle 101 in the current frame. The estimated value Vself estimated by the estimating unit 112 is used to estimate the rotation angle of the host vehicle (S33).

[0049]

[0050] V=[v1,v2,...,v n ] T (v)

[0051] The speed calculation unit 113 calculates the absolute speed of all measurement points, more specifically, including those corresponding to three-dimensional objects, based on the absolute speed Vself of the host vehicle 101 estimated by the estimation unit 112. This absolute speed is negative when approaching the host vehicle, and positive when departing from the host vehicle. As described above, the absolute speed calculated by the speed calculation unit 113 is used for point cloud classification (S2).

[0052] <Estimation of the rotation angle of the host vehicle (S33)>

[0053] Figure 3A The movement of an object in three-dimensional space can be described by the translational motion components, specifically the movement speed vx in the X-axis direction, the Y-axis direction, and the Z-axis direction. Vy 、 v Specifically, the z and rotational motion components are represented by the six degrees of freedom of rotation angles X_angle, Y_angle, and Z_angle in the X-axis direction, the Y-axis direction, and the Z-axis direction. On the other hand, the movement of the vehicle 101 can be represented by the three degrees of freedom of the moving speed (vehicle speed) of the vehicle 101, the tire angle, and the installation position of the lidar. The vehicle 101 is one of the objects, so it can be assumed that there is a causal relationship between the above three degrees of freedom that represent the movement of the vehicle 101 and the above six degrees of freedom (the above translational motion components and the above rotational motion components) that represent the movement of the object in three-dimensional space. In addition, both the above translational motion components and the rotational motion components have a causal relationship with the above three degrees of freedom that represent the movement of the vehicle 101, so it can be assumed that there is a causal relationship between the above translational motion components and the above rotational motion components. Figure 3A Therefore, based on the translational motion component Vself of the host vehicle 101 calculated based on the road surface point cloud data, the rotational motion component of the host vehicle 101 should be estimated by utilizing the above-mentioned causal relationship and correlation relationship. Figure 3B This is a diagram showing an example of the Z-axis rotational motion component Z_angle estimated from vx and vy of the translational motion component Vself using multiple regression analysis. Figure 3B The vertical axis of the graph represents the estimated value (predicted value) of Z_angle, and the horizontal axis represents the measured value of Z_angle. Figure 3B As shown in FIG, the estimated value of the Z-axis rotational motion component Z_angle obtained by the multivariate regression analysis is substantially equal to the measured value, and it can be seen that the Z-axis rotational motion component Z_angle can be estimated from vx and vy of the translational motion component Vself. Figure 3BOnly the estimated result of the rotational motion component Z_angle of the vehicle 101 on the Z axis is shown, but the rotational motion component Y_angle of the vehicle 101 on the Y axis can also be estimated based on the vx and vz of the translational motion component Vself of the vehicle 101. The rotational motion component X_angle of the vehicle 101 on the X axis can also be estimated based on the vy and vz of the translational motion component Vself of the vehicle 101. It should be noted that the estimation method of the rotational motion component of the vehicle 101 is not limited to multiple regression analysis, and other methods such as machine learning can also be used. In addition, the motion model used during vehicle design can also be used to estimate the rotational motion component of the vehicle 101. In addition, the rotational motion component of the vehicle 101 can also be directly obtained using sensors such as gyroscope sensors.

[0054] <Offset Processing (S4), Scan Matching Processing (S5)>

[0055] First, the scan matching process ( S5 ) will be described. Figure 4 、 Figure 5A 、 Figure 5B 、 Figure 6A as well as Figure 6B is a diagram for explaining the scan matching process between the previous frame and the current frame. Figure 4 In FIG, the arrow on the vehicle 101 indicates the vehicle length direction (front-back direction of the vehicle body), that is, the X-axis direction. Figure 4 ) and height directions ( Figure 4 The direction from the depth to the front in the figure) represents the Y-axis direction and the Z-axis direction. It should be noted that the definitions of the X-axis direction, the Y-axis direction and the Z-axis direction are the same in other figures, so the markings of the X-axis, Y-axis and Z-axis are omitted in those figures. Figure 4 An example of a schematic diagram when observing the point cloud data obtained by the laser radar 5 at a past time point (time t1) from above (Z-axis direction) is shown in the figure. Areas N1 to N6 schematically represent the measurement point group corresponding to the stationary object, and more specifically, the position and size of the measurement point group. Stationary objects include the road surface of the road on which the vehicle 101 is traveling, walls set on the side of the road, structures such as separation strips, other vehicles parked on the shoulder of the road, etc. Areas M31 and M32 schematically represent the measurement point group corresponding to the moving object, and more specifically, the position and size of the measurement point group. It should be noted that the objects detected by the laser radar 5 will be referred to as objects below, including people. Therefore, in addition to moving vehicles such as cars and bicycles, moving objects also include moving people (pedestrians, etc.). The arrows marked on areas M31 and M32 indicate the moving direction of the moving object.

[0056] exist Figure 5AIt is schematically shown in Figure 4 The point cloud data is the static point cloud data classified from the point cloud data obtained by the laser radar 5 at a past time point (time t1). Figure 5B Schematically shows stationary point cloud data classified from point cloud data acquired by the laser radar 5 at a current time point (time t2) after a predetermined time point in the past (time t1).

[0057] The scan matching unit 115 first performs a predetermined scan matching process to make the static point cloud data of the previous frame ( Figure 5A ) and the static point cloud data of the current frame ( Figure 5B ) to estimate the azimuth angle difference and motion vector of the vehicle 101 at a predetermined time (between frames). In the above scan matching process, in more detail, the static point cloud data other than the road surface point cloud data of the previous frame and the current frame, i.e., the static point cloud data corresponding to the three-dimensional object (hereinafter referred to as static three-dimensional point cloud data), are aligned with each other. Figure 6A and Figure 6B In FIG, it is schematically shown that the static point cloud data of the previous frame ( Figure 5A ) and the static point cloud data of the current frame ( Figure 5B ) is aligned. Figure 6A In FIG, the stationary point cloud data of the previous frame is shown by a dotted line, and the stationary point cloud data of the current frame is shown by a solid line. The scan matching unit 115 searches for (estimates) the azimuth angle difference and motion vector of the host vehicle 101 such that the measurement point clusters N1 to N6 of the previous frame, shown by the dotted lines, overlap (or coincide) with the measurement point clusters N1 to N6 of the current frame, shown by the solid lines. Figure 6B The static point cloud data of the previous frame and the static point cloud data of the current frame after alignment are shown in FIG. Figure 6B In the figure, the angle MA represents the azimuth angle difference of the vehicle 101 between frames. The hollow arrow MV represents the motion vector of the vehicle 101 between frames. The white circle in the figure schematically represents the center of gravity of the vehicle 101. The scanning matching unit 115 solves the optimization problem of minimizing the deviation (error) between the positions of the measurement point group N1 to N6 of the previous frame after alignment and the positions of the measurement point group N1 to N6 of the current frame, and outputs the final search result (estimated result) of the azimuth angle difference and the motion vector of the vehicle 101. It should be noted that the three-dimensional point cloud data can also be converted into two-dimensional point cloud data represented by the XY coordinate system before performing the above-mentioned alignment to reduce the computational load.

[0058] In the offset process (S4), the scan matching unit 115 offsets the stationary point cloud data of the previous frame using the inter-frame motion vector and azimuth angle difference of the host vehicle 101 estimated in the scan matching process (S5) of the previous cycle (between the previous frame and the previous frame). In the scan matching process (S5) executed after the offset process (S4), the scan matching unit 115 uses the offset stationary point cloud data of the previous frame as the initial value to align it with the stationary point cloud data of the current frame. In this way, the motion vector and azimuth angle difference of the host vehicle 101 estimated by the scan matching process are used as initial values when solving the optimization problem in the next scan matching process.

[0059] <Own Position Estimation Process (S6)>

[0060] The own position estimating unit 116 accumulates the azimuth angle differences and movement vectors of the own vehicle 101 estimated by the scan matching process ( S5 ), and estimates the own position (traveling position of the own vehicle 101 ) based on the accumulated azimuth angle differences and movement vectors.

[0061] The above-described embodiment has the following effects.

[0062] (1) The position estimation device 50 comprises: a laser radar 5, which is mounted on the vehicle 101 and irradiates electromagnetic waves around the vehicle 101, and detects the external conditions around the moving body based on the reflected waves; a data acquisition unit 111, which acquires point cloud data representing the detection results of the laser radar 5 at each specified time, the detection results including position information of the measurement point on the surface of the object that receives the reflected waves and speed information representing the relative moving speed of the measurement point; an estimation unit 112, which acts as an extraction unit and extracts road surface point cloud data corresponding to the road surface from the point cloud data acquired by the data acquisition unit 111; a scan matching unit 115, which acts as a motion component calculation unit and calculates the translational motion component representing the movement amount and movement direction of the translational motion of the vehicle 101 based on the position information and speed information of the measurement point corresponding to the road surface point cloud data. Figure 3A vx, vy, vz), and the rotational motion component ( Figure 3A The system includes an own-position estimating unit 116, which serves as an updating unit and updates the position information indicating the position of the host vehicle 101 based on the translational and rotational motion components calculated by the scan matching unit 115. This allows the position of the host vehicle 101 to be estimated with high accuracy regardless of the environment surrounding the traveling host vehicle 101.

[0063] (2) The position estimation device 50 further includes: a speed calculation unit 113 that calculates the absolute movement speed of each of the plurality of measurement points corresponding to the point cloud data acquired by the data acquisition unit 111 based on the speed 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 stationary point cloud data whose absolute value of the absolute movement speed calculated by the speed calculation unit 113 is less than a predetermined speed and moving point cloud data whose absolute value is greater than the predetermined speed. The own position estimation unit 116 also offsets the stationary point cloud data at a time point in the past that is a predetermined time before the current time point based on the translational motion component and the rotational motion component calculated by the scan matching unit 115, and searches for the translational motion component and the rotational motion component that minimize the position error between the measurement points corresponding to the stationary point cloud data at the current time point and the offset stationary point cloud data, and updates the position information of the host vehicle 101 based on the translational motion component and the rotational motion component obtained by the search. Thus, when solving the optimization problem by the scan matching process, it is possible to appropriately set the initial value, and it is possible to suppress the acquisition of an erroneous local solution as the search result.

[0064] (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 based on the calculated translational motion component based on a predetermined correlation between the translational motion component and the rotational motion component. The above correlation is predetermined based on the installation position of the laser radar 5 on the host vehicle 101, the driving speed (moving speed) of the host vehicle 101, and the steering angle (tire angle) of the host vehicle 101. In this way, by obtaining the information used for the offset processing (S4) (moving speed and rotation angle of the host vehicle 101) from the current frame, the own position can be estimated with high accuracy even in scenes where it is difficult to find or there are no corresponding feature points between frames.

[0065] The above-mentioned embodiment can be modified in various ways. The following describes a modified example. In the above-mentioned embodiment, the laser radar 5 as a detector is mounted on a vehicle, irradiates electromagnetic waves into the three-dimensional space around the vehicle, and detects the external conditions around the vehicle based on the reflected waves. However, the detector may be a device other than a laser radar such as a radar. For example, it may be a 4D imaging radar that obtains four-dimensional information (point cloud data) of the distance, azimuth, elevation and relative moving speed of the measurement point on the surface of an object contained in the three-dimensional space by irradiating millimeter wave radio waves and receiving reflected waves. In addition, the mobile body on which the detector is mounted may also be a mobile body other than a vehicle such as a self-propelled robot.

[0066] In the above embodiment, the estimating unit 112 as the information acquiring unit uses the static point cloud data ( Figure 5A ) and the static point cloud data of the current frame ( Figure 5B) performs alignment, thereby estimating the azimuth angle difference and movement vector of the vehicle 101 at a specified time (between frames). However, the information acquisition unit may also perform the above-mentioned alignment after converting the three-dimensional stationary point cloud data into two-dimensional stationary point cloud data represented by an XY coordinate system. In addition, the information acquisition unit may also use not only the stationary point cloud data of a past time point (time t1) but also the stationary point cloud data of multiple time points in the past, that is, not only the stationary point cloud data of the previous frame but also the stationary point cloud data of multiple frames in the past for the above-mentioned alignment. In this way, by using the stationary point cloud data of multiple frames in the past, even when the stationary object in front of the vehicle 101 is temporarily blocked by other vehicles, etc., the above-mentioned alignment can be performed well, which can improve the robustness.

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

[0068] The above description is merely an example, and the above embodiment and modifications do not limit the present invention unless the characteristics of the present invention are impaired. One or more of the above embodiment and modifications can be arbitrarily combined, and modifications can also be combined with each other.

[0069] According to the present invention, a mobile body can estimate its own position with high accuracy regardless of its surrounding environment.

[0070] The present invention has been described above with reference to preferred embodiments. However, it should be understood by those skilled in the art that various modifications and changes can be made without departing from the scope of the claims.

Claims

1. A position estimation device, characterized in that: have: A detector (5) is mounted on a moving body, irradiates electromagnetic waves around the moving body, and detects external conditions around the moving body based on reflected waves; a data acquisition unit (111) for acquiring point cloud data at each predetermined time representing a detection result of the detector (5), the detection result including position information of a measurement point on the surface of the object from which the reflected wave is obtained and speed information representing a relative moving speed of the measurement point; an extraction unit (112) for extracting road surface point cloud data corresponding to a road surface from the point cloud data acquired by the data acquisition unit (111); a motion component calculation unit (115) for calculating a translational motion component representing a movement amount and a movement direction of the translational motion of the mobile body and a rotational motion component representing a rotation amount and a rotation direction of the rotational motion of the mobile body based on the position information and the speed information of the measurement point corresponding to the road surface point cloud data; as well as An updating unit (116) updates position information indicating the position of the moving object based on the translational motion component and the rotational motion component calculated by the motion component calculating unit (115).

2. The position estimation device according to claim 1, wherein: Also features: a speed calculation unit (113) for calculating the absolute moving speed of each of the plurality of measurement points corresponding to the point cloud data based on the speed information of the measurement points and the translational motion component of the moving body; and a classification unit (114) for classifying the point cloud data into stationary point cloud data in which the absolute value of the absolute moving speed calculated by the speed calculation unit (113) is less than a predetermined speed and moving point cloud data in which the absolute value is greater than the predetermined speed, The updating unit (116) further performs an offset on the stationary point cloud data at a time point in the past that is a predetermined time before the current time point based on the translational motion component and the rotational motion component calculated by the motion component calculating unit (115), and searches for the translational motion component and the rotational motion component that minimize the positional error between the measurement points corresponding to the stationary point cloud data at the current time point and the stationary 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. The position estimation device according to claim 1, wherein: The motion component calculation unit (115) calculates the translational motion component of the moving body based on the position information and the speed information of the measurement point corresponding to the road surface point cloud data, and calculates the rotational motion component based on the calculated translational motion component based on a predetermined correlation between the translational motion component and the rotational motion component.

4. The position estimation device according to claim 3, wherein: The correlation is predetermined based on the installation position of the detector (5), the moving speed of the moving body, and the steering angle of the moving body.

5. The position estimating device according to any one of claims 1 to 4, characterized in that: The detector (5) is a laser radar or a radar.

Images