A dynamic scene positioning method and device based on a known point cloud map

Abstract

The application discloses a dynamic scene positioning method and device based on a known point cloud map, and the method comprises the following steps: acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different time points, and generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; for any current frame of point cloud data: determining the current frame of inertial measurement data corresponding to the current frame of point cloud data; estimating the observation pose of the robot based on the current frame of inertial measurement data and the current frame of point cloud data, and outputting a current pose estimation value; adjusting the pose corresponding to the current frame of point cloud data based on the current pose estimation value, and obtaining target frame point cloud data; performing dynamic point cloud filtering processing on the target frame point cloud data, and outputting static frame point cloud data; performing global optimization processing based on the static frame point cloud data and historical point cloud data of the target dynamic scene, and outputting the current optimal observation pose of the robot. Therefore, the consistency of repeated positioning in the target dynamic scene can be ensured.

Description

Technical Field

[0001] This invention belongs to the field of point cloud data processing technology, and particularly relates to a dynamic scene positioning method and device based on a known point cloud map. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) is a technology that builds maps and determines its own location in real time in uncertain environments. It collects environmental data using sensors (such as cameras and LiDAR) and extracts feature points from this data to construct a map. SLAM can also locate itself within the constructed map based on surrounding environmental data.

[0003] In related technologies, SLAM technology can only solve localization problems in static environments. It is not suitable for dynamic environments, especially scenarios with many moving objects (such as pedestrians, vehicles, and animals); this is because the movement of these objects interferes with SLAM localization, reducing its accuracy. Therefore, traditional SLAM-based localization methods struggle to provide high-precision localization results in dynamic environments. Summary of the Invention

[0004] To address the aforementioned problems in existing technologies, embodiments of the present invention provide a dynamic scene localization method and apparatus based on a known point cloud map. This method enables consistent localization in dynamic environments, thereby improving the localization accuracy of lidar for dynamic scenes under dynamic interference conditions.

[0005] According to a first aspect of the present invention, a dynamic scene localization method based on a known point cloud map is provided. The method includes: acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, generating several frames of point cloud data and several frames of inertial measurement data; for any current frame of point cloud data in the several frames of point cloud data: determining the current frame of inertial measurement data corresponding to the current frame of point cloud data based on the current time; estimating the robot's observed pose based on the current frame of inertial measurement data and the current frame of point cloud data, and outputting a current pose estimation value; adjusting the pose corresponding to the current frame of point cloud data based on the current pose estimation value to obtain target frame point cloud data; performing dynamic point cloud filtering processing on the target frame of point cloud data to output static frame point cloud data; and performing global optimization processing on the robot's observed pose based on the static frame point cloud data and historical point cloud data of the target dynamic scene to output the current optimal observed pose.

[0006] Optionally, the step of estimating the robot's observed pose based on the current frame inertial measurement data and the current frame point cloud data, and outputting the current pose estimate, includes: obtaining the previous predicted pose corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; predicting the robot's observed pose at the current moment based on the previous predicted pose and the current frame inertial measurement data, and outputting the current predicted pose; and correcting the current predicted pose based on the difference between the observed value corresponding to the current frame point cloud data and the expected observed value corresponding to the current predicted pose, thereby generating the current pose estimate.

[0007] Optionally, the step of performing dynamic point cloud filtering on the target frame point cloud data to output static frame point cloud data includes: performing rasterization on the target frame point cloud data to output several raster cells of the same size; for any current raster cell among the several raster cells: determining the number of points located in the current raster cell; determining the current height variance of the current raster cell based on the number of points, the height value of each point, and the average height of all points; obtaining the height variance corresponding to the current raster cell in the previous frame point cloud data adjacent to the current frame point cloud data; if the change in height variance between the current height variance and the previous height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, then determining the current raster cell as a dynamic raster; deleting each of the dynamic raster cells from the target frame point cloud data to obtain static frame point cloud data.

[0008] Optionally, determining the number of points located within the current grid cell includes: acquiring grid point cloud data corresponding to the current grid cell; for any target point in the grid point cloud data: acquiring the x-coordinate and y-coordinate values ​​of the target point; if both the x-coordinate and y-coordinate values ​​of the target point fall within the current grid cell, then determining that the target point is located within the current grid cell and identifying the target point as a grid point; if at least one of the x-coordinate and y-coordinate values ​​of the target point does not fall within the current grid cell, then determining that the target point is not located within the current grid cell; and determining the number of points located within the current grid cell based on the total number of grid points in the grid point cloud data.

[0009] Optionally, the step of performing global optimization processing on the robot's observation pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and outputting the current optimal observation pose, includes: performing global optimization processing on the robot's observation pose using a nonlinear optimization algorithm based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to obtain a constraint residual; and determining the current optimal observation pose when the constraint residual is minimized.

[0010] Optionally, when the historical point cloud data is a SLAM map, the step of using a nonlinear optimization algorithm to perform global optimization processing on the robot's observed pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to obtain constrained residuals includes: acquiring a SLAM map of the target dynamic scene; and aligning the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain global map registration residuals.

[0011] Optionally, when the historical point cloud data includes a SLAM map and historical frame point cloud data, the step of performing global optimization processing on the robot's observation pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and outputting the current optimal observation pose, includes: acquiring a SLAM map of the target dynamic scene; aligning the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain a global map registration residual; and / or selecting consecutive key frame point cloud data from the historical frame point cloud data that are located before the current frame point cloud data and have target relevance to the current frame point cloud data; performing registration calculations on the static frame point cloud data and the consecutive key frame point cloud data to generate a key frame registration residual; and / or performing similarity recognition processing on the current frame point cloud data and historical point cloud data based on a loop closure detection algorithm to generate loop closure frame point cloud data; and registering the static frame point cloud data with the loop closure frame point cloud data to generate a loop closure detection registration residual.

[0012] According to a second aspect of the present invention, a dynamic scene localization device based on a known point cloud map is also provided, characterized in that it includes: a data acquisition module, used to acquire laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, and generate several frames of point cloud data and several frames of inertial measurement data; a pose optimization module, used for any current frame point cloud data in the several frames of point cloud data: based on the current time, determining the current frame inertial measurement data corresponding to the current frame point cloud data; estimating the robot's observation pose based on the current frame inertial measurement data and the current frame point cloud data, and outputting a current pose estimation value; adjusting the pose corresponding to the current frame point cloud data based on the current pose estimation value to obtain target frame point cloud data; performing dynamic point cloud filtering processing on the target frame point cloud data, and outputting static frame point cloud data; and performing global optimization processing on the robot's observation pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and outputting the current optimal observation pose.

[0013] According to a third aspect of the present invention, an electronic device is also provided, the electronic device comprising: a processor; a memory for storing executable instructions of the processor; the processor being configured to read the executable instructions from the memory and execute the instructions to implement the method as described in the first aspect.

[0014] According to a fourth aspect of the present invention, a computer-readable medium is also provided, on which a computer program is stored, wherein the program, when executed by a processor, implements the method described in the first aspect.

[0015] This invention provides a method and apparatus for dynamic scene localization based on a known point cloud map. The method includes: first, acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, generating several frames of point cloud data and several frames of inertial measurement data; second, for any current frame of point cloud data in the several frames of point cloud data: based on the current time, determining the current frame of inertial measurement data corresponding to the current frame of point cloud data; estimating the robot's observed pose based on the current frame of inertial measurement data and the current frame of point cloud data, and outputting a current pose estimate; adjusting the pose corresponding to the current frame of point cloud data based on the current pose estimate, obtaining target frame point cloud data; performing dynamic point cloud filtering processing on the target frame of point cloud data, and outputting static frame point cloud data; and performing global optimization processing on the robot's observed pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and outputting the current optimal observed pose. This embodiment eliminates the impact of dynamic obstacles on the positioning accuracy of laser point cloud data by performing dynamic point cloud filtering on the laser point cloud data in dynamic target scenes; then, it combines historical point cloud data to perform global pose optimization on the static point cloud data after dynamic point cloud filtering; thus, it can ensure the consistency of repeated positioning in dynamic target scenes. Attached Figure Description

[0016] The following sections will describe some specific embodiments of the invention in detail by way of example and not limitation, with reference to the accompanying drawings. The same reference numerals in the drawings denote the same or similar parts or portions. Those skilled in the art should understand that these drawings are not necessarily drawn to scale. In the drawings:

[0017] Figure 1 This is a flowchart illustrating a dynamic scene localization method based on a known point cloud map according to an embodiment of the present invention.

[0018] Figure 2 This is a schematic diagram of the process for determining the current optimal observation pose in one embodiment of the present invention;

[0019] Figure 3 This is a schematic diagram of the structure of a dynamic scene positioning device based on a known point cloud map provided in an embodiment of the present invention. Detailed Implementation

[0020] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0021] like Figure 1 The diagram shown is a flowchart illustrating a dynamic scene localization method based on a known point cloud map provided in an embodiment of the present invention.

[0022] A dynamic scene localization method based on a known point cloud map; comprising at least the following steps:

[0023] S101, acquire laser point cloud data and inertial measurement data corresponding to the target dynamic scene at different times, and generate several frames of point cloud data and several frames of inertial measurement data.

[0024] S102, for any current frame point cloud data in several frames of point cloud data: based on the current time, determine the current frame inertial measurement data corresponding to the current frame point cloud data; based on the current frame inertial measurement data and the current frame point cloud data, estimate the robot's observed pose and output the current pose estimate; adjust the pose corresponding to the current frame point cloud data based on the current pose estimate to obtain the target frame point cloud data; perform dynamic point cloud filtering on the target frame point cloud data and output static frame point cloud data; based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, perform global optimization processing on the robot's observed pose and output the current optimal observed pose.

[0025] In S101, the robot acquires laser point cloud data corresponding to the dynamic scene of the target at different times using a lidar, resulting in several frames of point cloud data. The robot also acquires inertial measurement data corresponding to the dynamic scene of the target at different times using an inertial measurement sensor. Inertial measurement data typically refers to data acquired through an inertial measurement unit (IMU), which is primarily used to measure the motion state of an object, including information such as acceleration and angular velocity.

[0026] Each frame of point cloud data or each frame of inertial measurement data has a corresponding timestamp.

[0027] In S102, based on the current time corresponding to the current frame point cloud data, the current frame inertial measurement data is selected from several frames of inertial measurement data. Based on the current frame inertial measurement data and the current frame point cloud data, the robot's observed pose is estimated according to preset rules or a trained model to determine the robot's current pose estimate. For example, based on the current frame inertial measurement data and the current frame point cloud data, the Kalman filter method is used to estimate the robot's observed pose, and the robot's current pose estimate is output.

[0028] For example, estimating the robot's observed pose based on the current frame inertial measurement data and the current frame point cloud data, and outputting the current pose estimate, includes: obtaining the previous predicted pose corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; predicting the robot's observed pose at the current moment based on the previous predicted pose and the current frame inertial measurement data, and outputting the current predicted pose; and correcting the current predicted pose based on the difference between the observed value corresponding to the current frame point cloud data and the expected observed value corresponding to the current predicted pose, thereby generating the current pose estimate.

[0029] For example, the current pose estimate is calculated using the Kalman filter algorithm. The specific process is as follows:

[0030] (1) Prediction steps: Based on the previous predicted pose and the current frame inertial measurement data (e.g., acceleration and angular velocity), the robot's observed pose at the current moment is predicted using the motion model, and the current predicted pose is output; The motion model is shown in Equation (1):

[0031]

[0032] Equation (1);

[0033] in, For the current predicted pose, For the previous predicted pose, This is the inertial measurement data for the current frame. This is process noise.

[0034] (2) Update steps: Match the current frame point cloud data with the current predicted pose to correct the current predicted pose and generate the current pose estimate; the calculation formula for the current pose estimate is shown in equation (2):

[0035] Equation (2);

[0036] in, Used to indicate the current pose estimate; These are the observations obtained from the point cloud data of the current frame. These are the expected observations calculated based on the current predicted pose. For Kalman gain.

[0037] For example, the step of performing dynamic point cloud filtering on the target frame point cloud data to output static frame point cloud data includes: performing rasterization on the target frame point cloud data to output several raster cells of the same size; for any current raster cell among the several raster cells: determining the number of points located in the current raster cell; determining the current height variance of the current raster cell based on the number of points, the height value of each point, and the average height of all points; obtaining the height variance corresponding to the current raster cell in the previous frame point cloud data adjacent to the current frame point cloud data; if the change in height variance between the current height variance and the previous height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, then determining the current raster cell as a dynamic raster; deleting each of the dynamic raster cells from the target frame point cloud data to obtain static frame point cloud data.

[0038] Further, determining the number of points located within the current grid cell includes: acquiring grid point cloud data corresponding to the current grid cell; for any target point in the grid point cloud data: acquiring the x-coordinate and y-coordinate values ​​of the target point; if both the x-coordinate and y-coordinate values ​​of the target point fall within the current grid cell, then the target point is determined to be located within the current grid cell, and the target point is identified as a grid point; if at least one of the x-coordinate and y-coordinate values ​​of the target point does not fall within the current grid cell, then the target point is determined not to be located within the current grid cell; and determining the number of points located within the current grid cell based on the total number of grid points in the grid point cloud data.

[0039] Specifically, in order to remove dynamic obstacles from laser point cloud data, the laser point cloud data is first rasterized, and then it is determined whether it is a dynamic raster based on the number of points in each raster cell and the height variance.

[0040] For example: (1) Rasterization: The target frame point cloud data is rasterized to output several raster units of the same size, each raster unit containing a certain number of point cloud data; assuming the number of points in the current raster unit is The number of point clouds in the current raster cell can be calculated using the following formula (3):

[0041] Equation (3);

[0042] in, Indicates the current raster cell Gj, Indicates the first cell in the current grid. There are target points, if the target points are... In the current grid cell If the value is within the range, it is 1; otherwise, it is 0.

[0043] (2) Dynamic obstacle detection: For each current grid cell, calculate the current height variance of the current grid cell. As shown in equation (4):

[0044] Equation (4);

[0045] in, It is the current grid cell The Middle The height value of each target point. It is the current grid cell The average height of all target points in the range. Used to indicate the current grid cell The number of all points within the current grid cell. Point cloud quantity and height variance change Δ If all values ​​exceed the set threshold, the grid cell is considered to contain dynamic obstacles.

[0046] (3) Dynamic obstacle removal: For each target frame point cloud data, dynamic obstacles are removed using the above judgment method. If the current grid cell is a dynamic grid, the dynamic grid is removed from the target frame point cloud data.

[0047] For example, the step of performing global optimization processing on the robot's observation pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and outputting the current optimal observation pose, includes: performing global optimization processing on the robot's observation pose using a nonlinear optimization algorithm based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to obtain a constraint residual; and determining the current optimal observation pose when the constraint residual is minimized.

[0048] Specifically, based on static frame point cloud data and historical point cloud data of the target dynamic scene, a nonlinear optimization algorithm is used to perform global optimization processing on the robot's observation pose to obtain constraint residuals and optimization objective function; when the constraint residuals are minimized, the current optimal observation pose is determined according to the optimization objective function.

[0049] It should be noted that the target dynamic scene's historical point cloud data includes SLAM maps and / or continuous keyframe point cloud data, and / or loopback frame point cloud data. Specifically, the continuous keyframe point cloud data indicates frame point cloud data that precedes the current frame point cloud data and has target relevance to the current frame point cloud data; the loopback frame point cloud data indicates frame point cloud data obtained by performing similarity recognition processing on the current frame point cloud data and historical point cloud data.

[0050] Furthermore, when the historical point cloud data is a SLAM map, the step of using a nonlinear optimization algorithm to perform global optimization processing on the robot's observed pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to obtain constrained residuals includes: acquiring a SLAM map of the target dynamic scene; and aligning the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain global map registration residuals.

[0051] Specifically, the static frame point cloud data is aligned with the SLAM map using a global map matching algorithm to obtain the global map registration residual. and the corresponding optimization objective function When the constraint residual is minimized, the current optimal observation pose is determined according to the optimization objective function.

[0052] Global map registration residuals and the corresponding optimization objective function As shown in equation (5);

[0053]

[0054] Equation (5);

[0055] in, It is static frame point cloud data. It is SLAM map point cloud data.

[0056] This embodiment addresses the following for any current frame point cloud data in the laser point cloud data: Kalman filtering is performed on the current frame point cloud data and the corresponding inertial measurement data to output the robot's observed pose estimate at the current moment. After determining the current pose estimate, the current frame point cloud data is transformed to the pose corresponding to the current pose estimate, generating the target frame point cloud data. Then, dynamic point cloud filtering is applied to the target frame point cloud data to output static frame point cloud data. This eliminates the interference of dynamic obstacles on the positioning system in a dynamic environment, thereby improving the robot's accuracy in locating the target object. Finally, based on historical point cloud data of the dynamic scene, a nonlinear optimization method is used to globally optimize the static frame point cloud data, outputting the robot's current optimal observed pose at the current moment. This achieves consistency in repeated target object localization in dynamic scenes, further improving the robot's accuracy in locating the target object.

[0057] like Figure 2 The diagram shown is a flowchart illustrating the process of determining the current optimal observation pose in one embodiment of the present invention.

[0058] Determining the current optimal observation pose includes at least the following steps:

[0059] S201, Obtain the SLAM map of the target dynamic scene; Based on the global map matching algorithm, align the static frame point cloud data with the SLAM map to obtain the global map registration residual;

[0060] S202: Select continuous key frame point cloud data from historical frame point cloud data that are located before the current frame point cloud data and have target relevance to the current frame point cloud data; perform registration calculations on static frame point cloud data and continuous key frame point cloud data to generate key frame registration residuals.

[0061] S203, Based on the loop closure detection algorithm, perform similarity recognition processing on the current frame point cloud data and historical point cloud data to generate loop closure frame point cloud data; register the static frame point cloud data with the loop closure frame point cloud data to generate loop closure detection registration residual;

[0062] S204: After weighting the global map registration residual, keyframe registration residual, and loop closure detection registration residual, a weighted residual is obtained. When the weighted residual is minimized, the robot's current optimal observation pose is determined according to the objective optimization function.

[0063] In S202, static frame point cloud data is registered with continuous key frame point cloud data. The key frame registration residual corresponding to the static frame point cloud data is calculated by ICP and the key frame registration residual is used as a constraint term. The calculation formula of the key frame registration residual is shown in Equation (6).

[0064] Equation (6);

[0065] in, It is static frame point cloud data. It is continuous keyframe point cloud data.

[0066] In S203, the similarity between the current frame point cloud data and the historical point cloud data is identified by the loop closure detection algorithm to generate loop closure frame point cloud data; the loop closure detection registration error between the static frame point cloud data and the loop closure frame point cloud data is calculated by ICP, and the loop closure detection registration error is used as a constraint; the calculation formula for the loop closure detection registration error is shown in Equation (7):

[0067]

[0068] Equation (7);

[0069] in, It is static frame point cloud data. It is loopback frame point cloud data.

[0070] In S204, the joint nonlinear optimization objective function is shown in equation (8);

[0071] The ultimate joint optimization objective is to minimize the sum of residuals across all constraints:

[0072]

[0073] Equation (8);

[0074] in, These are the weight coefficients of each constraint term, which are comprehensively optimized to obtain the robot's current optimal observation pose at the current moment.

[0075] Therefore, this embodiment uses a combined nonlinear optimization method for global optimization to eliminate the interference of dynamic obstacles in the laser point cloud data and optimize the pose of the current frame point cloud data; all constraints are constructed using the ICP algorithm.

[0076] The following section will provide a detailed description of the dynamic scene localization method based on a known point cloud map provided in this embodiment, taking into account specific application scenarios.

[0077] S1: Acquire laser point cloud data and inertial measurement data corresponding to the target dynamic scene at different times, and generate several frames of point cloud data and several frames of inertial measurement data.

[0078] S2, for any current frame point cloud data in the plurality of frame point cloud data: based on the current time, determine the current frame inertial measurement data corresponding to the current frame point cloud data;

[0079] S3, obtain the previous predicted pose corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; based on the previous predicted pose and the current frame inertial measurement data, predict the robot's observed pose at the current moment and output the current predicted pose; based on the difference between the observed value corresponding to the current frame point cloud data and the expected observed value corresponding to the current predicted pose, correct the current predicted pose and generate the current pose estimate.

[0080] S4, adjust the pose corresponding to the current frame point cloud data based on the current pose estimation value to obtain the target frame point cloud data;

[0081] S5, perform rasterization processing on the target frame point cloud data to output several raster units of the same size; for any current raster unit among the several raster units: determine the number of points located in the current raster unit; based on the number of points, the height value of each point, and the average height of all points, determine the current height variance of the current raster unit; obtain the height variance corresponding to the current raster unit in the previous frame point cloud data adjacent to the current frame point cloud data; if the change in height variance between the current height variance and the previous height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, then determine that the current raster unit is a dynamic raster; delete each of the dynamic raster units from the target frame point cloud data to obtain static frame point cloud data.

[0082] S6, acquire the SLAM map of the target dynamic scene; align the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain a global map registration residual; and, select consecutive keyframe point cloud data from the historical frame point cloud data that are located before the current frame point cloud data and have target relevance to the current frame point cloud data; perform registration calculations on the static frame point cloud data and the consecutive keyframe point cloud data to generate a keyframe registration residual; and, perform similarity recognition processing on the current frame point cloud data and historical point cloud data based on a loop closure detection algorithm to generate loop closure frame point cloud data; register the static frame point cloud data with the loop closure frame point cloud data to generate a loop closure detection registration residual.

[0083] S7. When the constraint residual is minimized, determine the current optimal observation pose.

[0084] This embodiment, based on dynamic point cloud filtering technology and a known environmental point cloud map, not only solves the problem of high-precision positioning based on lidar under dynamic interference environment, but also solves the problem of consistency in repeated positioning based on a known point cloud map.

[0085] The method described in this embodiment is applicable to high-precision positioning tasks that encounter dynamic obstacles in a known fixed environment.

[0086] like Figure 3 The diagram shown is a structural schematic of a dynamic scene positioning device based on a known point cloud map provided in an embodiment of the present invention.

[0087] A dynamic scene localization device based on a known point cloud map, the device 300 includes: a data acquisition module 301, used to acquire laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, generating several frames of point cloud data and several frames of inertial measurement data; and a pose optimization module 302, used for any current frame of point cloud data in the several frames of point cloud data to: determine the current frame of inertial measurement data corresponding to the current frame of point cloud data based on the current time; estimate the robot's observation pose based on the current frame of inertial measurement data and the current frame of point cloud data, and output the current pose estimate; adjust the pose corresponding to the current frame of point cloud data based on the current pose estimate to obtain target frame point cloud data; perform dynamic point cloud filtering processing on the target frame of point cloud data to output static frame point cloud data; and perform global optimization processing on the robot's observation pose based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to output the current optimal observation pose.

[0088] In a preferred embodiment of this example, the pose optimization module includes: an acquisition unit, configured to acquire the previous predicted pose corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; a prediction unit, configured to predict the robot's observed pose at the current moment based on the previous predicted pose and the current frame inertial measurement data, and output the current predicted pose; and a pose correction unit, configured to correct the current predicted pose based on the difference between the observed value corresponding to the current frame point cloud data and the expected observed value corresponding to the current predicted pose, and generate a current pose estimate.

[0089] In a preferred embodiment of this example, the pose optimization module includes: a grid unit, used to perform rasterization processing on the target frame point cloud data and output several grid units of the same size; a first determination unit, used for any current grid unit among the several grid units to: determine the number of points located in the current grid unit; determine the current height variance of the current grid unit based on the number of points, the height value of each point, and the average height of all points; obtain the height variance corresponding to the current grid unit in the previous frame point cloud data adjacent to the current frame point cloud data; if the change in height variance between the current height variance and the previous height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, then determine the current grid unit as a dynamic grid; and a deletion unit, used to delete each of the dynamic grids from the target frame point cloud data to obtain static frame point cloud data.

[0090] In a preferred embodiment of this example, the determining unit includes: an acquisition subunit, used to acquire the grid point cloud data corresponding to the current grid unit; a first determining subunit, used for any target point in the grid point cloud data to: acquire the x-coordinate value and y-coordinate value of the target point; if both the x-coordinate value and y-coordinate value of the target point fall within the current grid unit, then the target point is determined to be located within the current grid unit, and the target point is determined to be a grid point; if at least one of the x-coordinate value and y-coordinate value of the target point does not fall within the current grid unit, then the target point is determined not to be located within the current grid unit; and a second determining subunit, used to determine the number of points located within the current grid unit based on the number of all grid points in the grid point cloud data.

[0091] In a preferred embodiment of this example, the pose optimization module further includes: a global optimization unit, used to perform global optimization processing on the robot's observed pose using a nonlinear optimization algorithm based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, to obtain a constraint residual; and a second determination unit, used to determine the current optimal observed pose when the constraint residual is minimized.

[0092] In a preferred embodiment of this example, when the historical point cloud data is a SLAM map, the global optimization unit includes: an acquisition subunit for acquiring a SLAM map of the target dynamic scene; and a data alignment subunit for aligning the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain a global map registration residual.

[0093] In a preferred embodiment of this example, when the historical point cloud data includes a SLAM map and historical frame point cloud data, the global optimization unit includes: a first acquisition subunit, used to acquire a SLAM map of the target dynamic scene; align the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain a global map registration residual; and / or, a second acquisition subunit, used to select consecutive key frame point cloud data located before the current frame point cloud data and having target relevance with the current frame point cloud data from the historical frame point cloud data; perform registration calculations on the static frame point cloud data and the consecutive key frame point cloud data to generate a key frame registration residual; and / or, a third acquisition subunit, used to perform similarity recognition processing on the current frame point cloud data and historical point cloud data based on a loop closure detection algorithm to generate loop closure frame point cloud data; and perform registration on the static frame point cloud data and the loop closure frame point cloud data to generate a loop closure detection registration residual.

[0094] The above-described apparatus can execute a dynamic scene localization method based on a known point cloud map provided in an embodiment of the present invention, and has the corresponding functional modules and beneficial effects for executing a dynamic scene localization method based on a known point cloud map. Technical details not described in detail in this embodiment can be found in the dynamic scene localization method based on a known point cloud map provided in an embodiment of the present invention.

[0095] The present invention also provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor being configured to read the executable instructions from the memory and execute the instructions to implement a dynamic scene localization method based on a known point cloud map as described in the present invention.

[0096] In addition to the methods and apparatus described above, embodiments of this application may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the steps in the methods according to various embodiments of this application described in the "Exemplary Methods" section above.

[0097] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0098] Furthermore, embodiments of this application may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the methods according to the following embodiments of this application described in the "Exemplary Methods" section above.

[0099] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof.

[0100] The basic principles of this application have been described above with reference to specific embodiments. However, it should be noted that the advantages, benefits, and effects mentioned in this application are merely examples and not limitations, and should not be considered as essential features of each embodiment of this application. Furthermore, the specific details disclosed above are for illustrative and facilitative purposes only, and are not limitations. These details do not limit the application to the necessity of employing the aforementioned specific details for implementation.

[0101] The block diagrams of devices, apparatuses, devices, and systems involved in this application are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these devices, apparatuses, devices, and systems can be connected, arranged, and configured in any manner. Words such as “comprising,” “including,” “having,” etc., are open-ended terms meaning “including but not limited to,” and are used interchangeably with them. The terms “or” and “and” as used herein refer to the terms “and / or,” and are used interchangeably with them unless the context clearly indicates otherwise. The term “such as” as used herein refers to the phrase “such as but not limited to,” and is used interchangeably with it.

[0102] It should also be noted that in the apparatus, equipment, and methods of this application, the components or steps can be disassembled and / or recombined. These disassemblies and / or recombinations should be considered as equivalent solutions of this application.

[0103] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use this application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this application. Therefore, this application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0104] The above description has been given for purposes of illustration and description. Furthermore, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

[0105] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0106] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0107] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A dynamic scene localization method based on a known point cloud map, characterized in that, The method comprises the following steps: acquiring laser point cloud data corresponding to a target dynamic scene at different time and inertial measurement data, and generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; for any current frame of point cloud data in the plurality of frames of point cloud data: based on the current time, determining the current frame of inertial measurement data corresponding to the current frame of point cloud data; acquiring the last predicted pose corresponding to the last frame of point cloud data adjacent to the current frame of point cloud data; based on the last predicted pose and the current frame of inertial measurement data, predicting the robot observation pose at the current time, and outputting the current predicted pose; based on the difference between the observation value corresponding to the current frame of point cloud data and the expected observation value corresponding to the current predicted pose, correcting the current predicted pose to generate a current pose estimate value; based on the current pose estimate value, adjusting the pose corresponding to the current frame of point cloud data to obtain target frame point cloud data; performing rasterization processing on the target frame point cloud data to output a plurality of grid cells of the same size; for any current grid cell in the plurality of grid cells: determining the number of points located in the current grid cell; based on the number of points, the height value of each point, and the height mean value of all points, determining the current height variance of the current grid cell; acquiring the height variance corresponding to the current grid cell in the last frame of point cloud data adjacent to the current frame of point cloud data; if the height variance change between the current height variance and the last height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, the current grid cell is determined to be a dynamic grid; deleting each dynamic grid from the target frame point cloud data to obtain a static frame point cloud data; acquiring a SLAM map of the target dynamic scene; based on a global map matching algorithm, aligning the static frame point cloud data with the SLAM map to obtain a global map registration residual; and, selecting continuous key frame point cloud data located before the current frame of point cloud data and having target relevance with the current frame of point cloud data from historical frame point cloud data; performing registration calculation on the static frame point cloud data and the continuous key frame point cloud data through ICP to generate a key frame registration residual; and, based on a loop detection algorithm, performing similarity identification processing on the current frame of point cloud data and historical point cloud data to generate a loop frame point cloud data; performing registration on the static frame point cloud data and the loop frame point cloud data through ICP to generate a loop detection registration residual; after weighting processing of the global map registration residual, the key frame registration residual and the loop detection registration residual, a weighted residual is obtained; when the weighted residual is the minimum, the current optimal observation pose of the robot is determined according to a target optimization function; the joint nonlinear optimization objective function is shown in formula (8); the final joint optimization objective is to minimize the residual of all constraints: formula (8); wherein, are weight coefficients of each constraint term, and the current optimal observation pose of the robot at the current time is obtained by comprehensive optimization.

2. The method of claim 1, wherein, the determination of the number of points located in the current grid cell comprises: acquiring the grid point cloud data corresponding to the current grid cell; For any target point in the grid point cloud data: obtain the horizontal coordinate value and the vertical coordinate value of the target point; if the horizontal coordinate value and the vertical coordinate value of the target point are both within the current grid unit, it is determined that the target point is located within the current grid unit, and the target point is determined as a grid point; if at least one of the horizontal coordinate value and the vertical coordinate value of the target point is not within the current grid unit, it is determined that the target point is not located within the current grid unit; Based on the number of all grid points in the grid point cloud data, the number of points located within the current grid unit is determined.

3. A dynamic scene localization apparatus based on a known point cloud map, the apparatus comprising: Comprise: The data acquisition module is used for acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different time instants, generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; The pose optimization module is used for, for any current frame of point cloud data in the plurality of frames of point cloud data: based on a current time instant, determining current frame inertial measurement data corresponding to the current frame of point cloud data; obtaining a last predicted pose corresponding to a last frame of point cloud data adjacent to the current frame of point cloud data; Based on the last predicted pose and the current frame inertial measurement data, a current predicted pose is predicted for a robot observation pose at the current time instant, and the current predicted pose is output; Based on a difference between an observed value corresponding to the current frame of point cloud data and an expected observed value corresponding to the current predicted pose, the current predicted pose is corrected to generate a current pose estimate value; Based on the current pose estimate value, the pose corresponding to the current frame of point cloud data is adjusted to obtain target frame point cloud data; the target frame point cloud data is subjected to grid processing to output a plurality of grid units of the same size; for any current grid unit in the plurality of grid units: the number of points located within the current grid unit is determined; based on the number of points, a height value of each point, and a height mean value of all points, a current height variance of the current grid unit is determined; a height variance corresponding to the current grid unit in a last frame of point cloud data adjacent to the current frame of point cloud data is obtained; if a height variance change between the current height variance and the last height variance is greater than a first preset threshold value and the number of points is greater than a second preset threshold value, the current grid unit is determined as a dynamic grid; each dynamic grid is deleted from the target frame point cloud data to obtain static frame point cloud data; based on the static frame point cloud data and historical point cloud data of the target dynamic scene, a global optimization process is performed on a robot observation pose to output a current optimal observation pose; The pose optimization module further comprises: a global optimization unit configured to perform a global optimization process on a robot observation pose based on the static frame point cloud data and historical point cloud data of the target dynamic scene by using a nonlinear optimization algorithm to obtain a constraint residual; and a second determination unit configured to determine a current optimal observation pose when the constraint residual is at a minimum. The global optimization unit comprises: a first obtaining subunit configured to obtain a SLAM map of a target dynamic scene; a second obtaining subunit configured to perform data alignment on the static frame point cloud data and the SLAM map based on a global map matching algorithm to obtain a global map registration residual; a third obtaining subunit configured to select, from historical frame point cloud data, continuous key frame point cloud data located before the current frame point cloud data and having target relevance with the current frame point cloud data; and an ICP configured to perform registration calculation on the static frame point cloud data and the continuous key frame point cloud data to generate a key frame registration residual; a fourth obtaining subunit configured to perform similarity identification processing on the current frame point cloud data and historical point cloud data based on a loop detection algorithm to generate loop frame point cloud data; and an ICP configured to perform registration on the static frame point cloud data and the loop frame point cloud data to generate a loop detection registration residual; a weighting unit configured to perform weighting processing on the global map registration residual, the key frame registration residual, and the loop detection registration residual to obtain a weighted residual; and a determination unit configured to determine a current optimal observation pose of the robot according to a target optimization function when the weighted residual is minimum. The joint nonlinear optimization objective function is shown in equation (8). The final joint optimization objective is to minimize the residual sum of all constraints: Equation (8). wherein, are weight coefficients of each constraint term, and the current optimal observation pose of the robot at the current time is obtained by comprehensive optimization.

4. An electronic device, the electronic device comprising: A processor; A memory for storing the executable instructions of the processor; The processor is configured to read the executable instructions from the memory and execute the instructions to implement the method according to any one of claims 1-2.

5. A computer readable medium having stored thereon a computer program, which, when executed by a processor, implements the method according to any one of claims 1-2.

Images