Dynamic scene positioning method and device based on known point cloud map

Abstract

The invention discloses a dynamic scene positioning method and device based on a known point cloud map, and the method comprises the steps: obtaining corresponding laser point cloud data and inertial measurement data of a target dynamic scene at different moments, and generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; for any current frame point cloud data, determining current frame inertia measurement data corresponding to the current frame point cloud data; estimating the observation pose of the robot based on the current frame inertia 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 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 of the robot. Therefore, the consistency of repeated positioning in the target dynamic scene can be ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of point cloud data processing, and in particular relates to a dynamic scene positioning method and device based on a known point cloud map. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) technology is a technology that builds maps and determines its own position in real time in an uncertain environment. It uses sensors (such as cameras, lidar, etc.) to collect environmental data and extract feature points in the environmental data to build a map; SLAM technology can also locate itself in the built map based on the surrounding environmental data.

[0003] In the related technologies, SLAM technology can only solve the positioning problem in static environments. It is not applicable to dynamic environments, especially scenes with a large number of moving objects (such as pedestrians, vehicles, animals, etc.); this is because the movement of moving objects will interfere with SLAM positioning and reduce the accuracy of SLAM positioning. For this reason, traditional SLAM-based positioning methods are difficult to provide high-precision positioning results in dynamic environments. Summary of the invention

[0004] In view of the above problems existing in the prior art, the embodiments of the present invention provide a method and device for dynamic scene positioning based on a known point cloud map. The method can achieve consistency of repeated positioning in a dynamic environment, thereby improving the positioning accuracy of the laser radar for the target dynamic scene in a dynamic interference environment.

[0005] According to a first aspect of an embodiment of the present invention, a dynamic scene positioning method based on a known point cloud map is provided, the method comprising: acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, 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 moment, determining the current frame of inertial measurement data corresponding to the current frame of point cloud data; estimating the observation posture of the robot based on the current frame of inertial measurement data and the current frame of point cloud data, and outputting a current posture estimation value; adjusting the posture corresponding to the current frame of point cloud data based on the current posture 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 on the observation posture of the robot 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 posture.

[0006] Optionally, the robot's observed posture is estimated based on the current frame inertial measurement data and the current frame point cloud data, and a current posture estimate is output; including: obtaining the previous predicted posture corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; predicting the robot's observed posture at the current moment based on the previous predicted posture and the current frame inertial measurement data, and outputting the current predicted posture; 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 posture, correcting the current predicted posture to generate a current posture estimate.

[0007] Optionally, the target frame point cloud data is subjected to dynamic point cloud filtering processing to output static frame point cloud data; including: rasterizing the target frame point cloud data to output a number of grid units of the same size; for any current grid unit among the several grid units: determining the number of points located in the current grid unit; determining the current height variance of the current grid unit based on the number of points, the height value of each point, and the mean height of all points; obtaining 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 height variance change 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, determining that the current grid unit is a dynamic grid; deleting each of the dynamic grids from the target frame point cloud data to obtain static frame point cloud data.

[0008] Optionally, the determining the number of points located within the current grid cell includes: obtaining grid point cloud data corresponding to the current grid cell; for any target point in the grid point cloud data: obtaining 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 both fall within the current grid cell, then determining that the target point is located within the current grid cell, and determining the target point as a grid point; if at least one of the horizontal coordinate value and the vertical coordinate value 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; based on the number of all grid points in the grid point cloud data, determining the number of points located within the current grid cell.

[0009] Optionally, the robot's observation posture is globally optimized based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and the current optimal observation posture is output; including: based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, a nonlinear optimization algorithm is used to globally optimize the robot's observation posture to obtain a constrained residual; when the constrained residual is minimized, the current optimal observation posture is determined.

[0010] Optionally, when the historical point cloud data is a SLAM map, the robot's observation posture is globally optimized based on the static frame point cloud data and the historical point cloud data of the target dynamic scene using a nonlinear optimization algorithm to obtain a constrained residual; including: obtaining 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.

[0011] Optionally, when the historical point cloud data includes a SLAM map and historical frame point cloud data, the robot's observation posture is globally optimized based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and the current optimal observation posture is output; including: obtaining 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 continuous key frame point cloud data that is located before the current frame point cloud data and has target relevance to the current frame point cloud data from the historical frame point cloud data; performing registration calculations on the static frame point cloud data and the continuous key frame point cloud data to generate key frame registration residuals; and / or, performing similarity recognition processing on the current frame point cloud data and the historical point cloud data based on a loop detection algorithm to generate loop frame point cloud data; aligning the static frame point cloud data with the loop frame point cloud data to generate loop detection registration residuals.

[0012] According to the second aspect of an embodiment of the present invention, a dynamic scene positioning 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 the target dynamic scene at different times, and generate a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; a posture optimization module, used for: for any current frame point cloud data in the plurality of frames of point cloud data: based on the current moment, determining the current frame inertial measurement data corresponding to the current frame point cloud data; estimating the robot's observation posture based on the current frame inertial measurement data and the current frame point cloud data, and outputting a current posture estimation value; adjusting the posture corresponding to the current frame point cloud data based on the current posture estimation value to obtain the 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 on the robot's observation posture 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 posture.

[0013] According to a third aspect of an embodiment of the present invention, there is further provided an electronic device, comprising: a processor; a memory for storing instructions executable by the processor; the processor for reading the executable instructions from the memory and executing the instructions to implement the method described in the first aspect.

[0014] According to a fourth aspect of an embodiment of the present invention, there is further provided a computer-readable medium on which a computer program is stored. When the program is executed by a processor, the method described in the first aspect is implemented.

[0015] An embodiment of the present invention provides a dynamic scene positioning method and device based on a known point cloud map, the method comprising: first, obtaining laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, and generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; second, 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; estimating the robot's observation posture based on the current frame of inertial measurement data and the current frame of point cloud data, and outputting a current posture estimation value; adjusting the posture corresponding to the current frame of point cloud data based on the current posture 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 on the robot's observation posture 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 posture. This embodiment can eliminate the influence of dynamic obstacles on the positioning accuracy of laser point cloud in target dynamic scenes by performing dynamic point cloud filtering on the laser point cloud data; then, the static point cloud data after dynamic point cloud filtering is globally optimized in combination with historical point cloud data; thereby, the consistency of repeated positioning in target dynamic scenes can be ensured. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Hereinafter, some specific embodiments of the present invention will be described in detail in an exemplary and non-limiting manner with reference to the accompanying drawings. The same reference numerals in the accompanying drawings indicate the same or similar components or parts. It should be understood by those skilled in the art that these drawings are not necessarily drawn to scale. In the accompanying drawings:

[0017] Figure 1 A schematic diagram of a flow chart of a dynamic scene positioning method based on a known point cloud map provided by an embodiment of the present invention;

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

[0019] Figure 3 A schematic diagram of the structure of a dynamic scene positioning device based on a known point cloud map provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0020] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0021] like Figure 1 , which is a flow chart of a dynamic scene positioning method based on a known point cloud map provided by an embodiment of the present invention.

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

[0023] S101, acquiring laser point cloud data and inertial measurement data corresponding to a target dynamic scene at different times, and generating a plurality of frames of point cloud data and a plurality of frames of inertial measurement data;

[0024] S102, for any current frame point cloud data among several frames of point cloud data: based on the current moment, 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 observation posture, and output the current posture estimate; based on the current posture estimate, adjust the posture corresponding to the current frame point cloud data 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 on the robot's observation posture, and output the current optimal observation posture.

[0025] In S101, laser point cloud data corresponding to the target dynamic scene at different times are obtained by using the laser radar on the robot to obtain a number of frames of point cloud data; inertial measurement data corresponding to the target dynamic scene at different times are obtained by using the inertial measurement sensor on the robot. Inertial measurement data usually refers to data obtained by an inertial measurement unit (IMU), which is mainly used to measure the motion state of an object, including acceleration, angular velocity and other information.

[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 moment corresponding to the current frame point cloud data, the current frame inertial measurement data is selected from a plurality of frames of inertial measurement data. According to the current frame inertial measurement data and the current frame point cloud data, the robot observation posture is estimated based on a preset rule or a training model to determine the current posture estimation value of the robot. For example: based on the current frame inertial measurement data and the current frame point cloud data, the robot observation posture is estimated using the Kalman filter method, and the current posture estimation value of the robot is output.

[0028] Exemplarily, the method estimates the observed posture of the robot based on the current frame inertial measurement data and the current frame point cloud data, and outputs the current posture estimate; includes: obtaining the previous predicted posture corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; predicting the observed posture of the robot at the current moment based on the previous predicted posture and the current frame inertial measurement data, and outputting the current predicted posture; 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 posture, correcting the current predicted posture to generate the current posture estimate.

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

[0030] (1) Prediction step: Based on the last predicted pose and the current frame inertial measurement data (e.g., acceleration and angular velocity), the motion model is used to predict the robot's observed pose at the current moment and output the current predicted pose; the motion model is shown in formula (1):

[0031]

[0032] in, is the current predicted pose, x t-1 is the last predicted pose, u t is the inertial measurement data of the current frame, w t is the process noise.

[0033] (2) Update step: Match the current frame point cloud data with the current predicted pose, thereby correcting the current predicted pose and generating the current pose estimate. The calculation formula of the current pose estimate is shown in formula (2):

[0034]

[0035] Among them, x t Used to indicate the current pose estimate; z t is the observation value obtained through the point cloud data of the current frame, is the expected observation value calculated based on the current predicted pose, K t is the Kalman gain.

[0036] Exemplarily, the target frame point cloud data is subjected to dynamic point cloud filtering processing to output static frame point cloud data; including: rasterizing the target frame point cloud data to output a plurality of grid units of the same size; for any current grid unit among the plurality of grid units: determining the number of points located in the current grid unit; determining the current height variance of the current grid unit based on the number of points, the height value of each point, and the mean height of all points; obtaining 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 height variance change 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, determining that the current grid unit is a dynamic grid; deleting each of the dynamic grids from the target frame point cloud data to obtain static frame point cloud data.

[0037] Further, the determination of the number of points located within the current grid cell includes: obtaining grid point cloud data corresponding to the current grid cell; for any target point in the grid point cloud data: obtaining 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 both fall within the current grid cell, then determining that the target point is located within the current grid cell, and determining the target point as a grid point; if at least one of the horizontal coordinate value and the vertical coordinate value 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; based on the number of all grid points in the grid point cloud data, determining the number of points located within the current grid cell.

[0038] Specifically, in order to remove dynamic obstacles from the laser point cloud data, the laser point cloud data is firstly rasterized, and then it is determined whether it is a dynamic grid according to the number of point clouds and height variance in each grid unit.

[0039] For example: (1) Rasterization: The target frame point cloud data is rasterized to output a number of grid cells of the same size, each of which contains a certain amount of point cloud data. Assume that the number of points in the current grid cell is Then the number of point clouds in the current grid cell can be calculated by the following formula (3):

[0040]

[0041] Among them, G j Represents the current grid cell Gj, p i represents the i-th target point in the current grid cell. If the target point p i In the current grid cell G j If it is within, the value is 1, otherwise it is 0.

[0042] (2) Dynamic obstacle judgment: For each current grid cell, calculate the current height variance S of the current grid cell h，j , as shown in the following formula (4):

[0043]

[0044] Among them, z i is the current grid cell G j The height value of the i-th target point in , is the current grid cell G j The average height of all target points in m j Used to indicate the current grid cell G j If the current grid cell G j The number of point clouds m j and the height variance change ΔS h，j If both exceed the set threshold, the grid cell is considered to contain a dynamic obstacle.

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

[0046] Exemplarily, the robot's observation posture is globally optimized based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, and the current optimal observation posture is output; including: based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, the robot's observation posture is globally optimized using a nonlinear optimization algorithm to obtain a constrained residual; when the constrained residual is minimized, the current optimal observation posture is determined.

[0047] Specifically, based on the static frame point cloud data and the historical point cloud data of the target dynamic scene, the robot's observation posture is globally optimized using a nonlinear optimization algorithm to obtain a constraint residual and an optimization objective function; when the constraint residual is minimized, the current optimal observation posture is determined according to the optimization objective function.

[0048] It should be noted that the historical point cloud data SLAM map of the target dynamic scene, and / or continuous key frame point cloud data, and / or loop frame point cloud data. Among them, the continuous key frame point cloud data is used to indicate the frame point cloud data that is located before the current frame point cloud data and has target relevance with the current frame point cloud data; the loop frame point cloud data is used to indicate the frame point cloud data obtained by performing similarity recognition processing on the current frame point cloud data and the historical point cloud data.

[0049] Furthermore, when the historical point cloud data is a SLAM map, the robot's observation posture is globally optimized based on the static frame point cloud data and the historical point cloud data of the target dynamic scene using a nonlinear optimization algorithm to obtain a constrained residual; including: obtaining 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.

[0050] Specifically, the static frame point cloud data is aligned with the SLAM map through the global map matching algorithm to obtain the global map registration residual r map And the corresponding optimization objective function When the constraint residual is the smallest, the current optimal observation posture is determined according to the optimization objective function.

[0051] Global map registration residual r map And the corresponding optimization objective function As shown in formula (5);

[0052]

[0053]

[0054] Among them, p i is the static frame point cloud data, p kown It is the SLAM map point cloud data.

[0055] This embodiment is directed to any current frame point cloud data in the laser point cloud data: Kalman filtering is performed based on the current frame point cloud data and the corresponding inertial measurement data, and the observed pose estimation value of the robot at the current moment is output. After determining the current pose estimation value, the current frame point cloud data is converted to the pose corresponding to the current pose estimation value to generate the target frame point cloud data; then, the target frame point cloud data is filtered based on the dynamic point cloud filtering process, and static frame point cloud data is output; thereby, the interference of dynamic obstacles to the positioning system in the target dynamic environment can be eliminated, thereby improving the accuracy of the robot's positioning of the target object. Finally, based on the historical point cloud data of the target dynamic scene, the static frame point cloud data is globally optimized using a nonlinear optimization method, and the current optimal observed pose corresponding to the robot at the current moment is output; thereby, the consistency of repeated positioning of the target object in the target dynamic scene can be achieved, and the accuracy of the robot's positioning of the target object can be improved.

[0056] like Figure 2 As shown, it is a schematic diagram of the process of determining the current optimal observation posture in one embodiment of the present invention.

[0057] Determining the current optimal observation posture includes at least the following steps:

[0058] S201, obtaining a SLAM map of a 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;

[0059] S202, selecting continuous key frame point cloud data that is located before the current frame point cloud data and has target relevance with the current frame point cloud data from the historical frame point cloud data; performing registration calculation on the static frame point cloud data and the continuous key frame point cloud data to generate a key frame registration residual;

[0060] S203, performing similarity recognition processing on the current frame point cloud data and the historical point cloud data based on the loop detection algorithm to generate loop frame point cloud data; registering the static frame point cloud data with the loop frame point cloud data to generate loop detection registration residual;

[0061] S204, performing weighted processing on the global map registration residual, the key frame registration residual and the loop detection registration residual to obtain a weighted residual; when the weighted residual is the smallest, determining the current optimal observation posture of the robot according to the target optimization function.

[0062] In S202, the static frame point cloud data is registered with the continuous key frame point cloud data, and 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 item; the calculation formula of the key frame registration residual is shown in formula (6);

[0063]

[0064] Among them, p i is the static frame point cloud data, p sub It is continuous keyframe point cloud data.

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

[0066]

[0067] Among them, p i is the static frame point cloud data, p loop It is the loop frame point cloud data.

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

[0069] The final joint optimization goal is to minimize the residual sum of all constraints:

[0070]

[0071] Among them, λ laser ,λ loop ,λ map It is the weight coefficient of each constraint item, and the current optimal observation posture of the robot at the current moment is obtained through comprehensive optimization.

[0072] Therefore, this embodiment combines the nonlinear optimization method to perform global optimization, eliminate the interference of dynamic obstacles in the laser point cloud data, and optimize the position and posture of the current frame point cloud data; all constraints are constructed through the ICP algorithm.

[0073] The following will describe in detail a dynamic scene positioning method based on a known point cloud map provided by this embodiment in conjunction with specific application scenarios.

[0074] S1, obtaining laser point cloud data and inertial measurement data corresponding to the target dynamic scene at different times, and generating several frames of point cloud data and several frames of inertial measurement data.

[0075] S2, for any current frame of point cloud data among the plurality of frames of point cloud data: based on the current moment, determining the current frame of inertial measurement data corresponding to the current frame of point cloud data;

[0076] S3, obtaining the previous predicted posture corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; based on the previous predicted posture and the current frame inertial measurement data, predicting the observed posture of the robot at the current moment, and outputting the current predicted posture; 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 posture, correcting the current predicted posture, and generating a current posture estimate.

[0077] S4, 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;

[0078] S5, rasterizing the target frame point cloud data and outputting a number of grid units of the same size; for any current grid unit among the several grid units: determining the number of points located in the current grid unit; determining the current height variance of the current grid unit based on the number of points, the height value of each point, and the mean height of all points; obtaining 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 height variance change 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, determining that the current grid unit is a dynamic grid; deleting each of the dynamic grids from the target frame point cloud data to obtain static frame point cloud data.

[0079] S6, obtain the SLAM map of the target dynamic scene; align the static frame point cloud data with the SLAM map based on the global map matching algorithm to obtain the global map registration residual; and, select the continuous key frame point cloud data that is located before the current frame point cloud data and has target relevance with the current frame point cloud data from the historical frame point cloud data; perform registration calculation on the static frame point cloud data and the continuous key frame point cloud data to generate key frame registration residuals; and, perform similarity recognition processing on the current frame point cloud data and the historical point cloud data based on the loop detection algorithm to generate loop frame point cloud data; align the static frame point cloud data with the loop frame point cloud data to generate loop detection registration residuals.

[0080] S7, when the constraint residual is the smallest, determine the current optimal observation posture.

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

[0082] The method of this embodiment is applicable to high-precision positioning tasks in a known fixed environment with interference from dynamic obstacles.

[0083] like Figure 3 , which is a schematic diagram of the structure of a dynamic scene positioning device based on a known point cloud map provided by an embodiment of the present invention.

[0084] A dynamic scene positioning 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, and generate a plurality of frames of point cloud data and a plurality of frames of inertial measurement data; a posture optimization module 302, used to determine, for any current frame of point cloud data in the plurality of frames of point cloud data: based on the current moment, the current frame of inertial measurement data corresponding to the current frame of point cloud data; based on the current frame of inertial measurement data and the current frame of point cloud data, estimate the robot's observation posture, and output a current posture estimation value; based on the current posture estimation value, adjust the posture corresponding to the current frame of point cloud data, and obtain target frame point cloud data; perform dynamic point cloud filtering processing 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 observation posture, and output the current optimal observation posture.

[0085] In a preferred implementation of this embodiment, the posture optimization module includes: an acquisition unit, used to acquire the previous predicted posture corresponding to the previous frame point cloud data adjacent to the current frame point cloud data; a prediction unit, used to predict the observed posture of the robot at the current moment based on the previous predicted posture and the current frame inertial measurement data, and output the current predicted posture; a posture correction unit, used to correct the current predicted posture 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 posture, and generate a current posture estimate.

[0086] In a preferred implementation manner of this embodiment, the posture optimization module includes: a grid unit, which is used to rasterize the target frame point cloud data and output a number of grid units of the same size; a first determination unit, which is used to determine, for any current grid unit among the several grid units: the number of points located in the current grid unit; based on the number of points, the height value of each point, and the mean height of all points, determine the current height variance of the current grid unit; 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 height variance change 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, determine that the current grid unit is a dynamic grid; a deletion unit, which is used to delete each of the dynamic grids from the target frame point cloud data to obtain static frame point cloud data.

[0087] In a preferred implementation manner of this embodiment, the determination unit includes: an acquisition subunit, used to acquire grid point cloud data corresponding to the current grid cell; a first determination subunit, used to, for any target point in the grid point cloud data: acquire 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 both fall within the current grid cell, then it is determined that the target point is located within the current grid cell, 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 does not fall within the current grid cell, then it is determined that the target point is not located within the current grid cell; a second determination subunit, used to determine the number of points located within the current grid cell based on the number of all grid points in the grid point cloud data.

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

[0089] In a preferred implementation of this embodiment, when the historical point cloud data is a SLAM map, the global optimization unit includes: an acquisition subunit, used to acquire the SLAM map of the target dynamic scene; a data alignment subunit, used to align the static frame point cloud data with the SLAM map based on a global map matching algorithm to obtain a global map alignment residual.

[0090] In a preferred implementation manner of this embodiment, 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 a target dynamic scene; based on a global map matching algorithm, the static frame point cloud data is aligned with the SLAM map to obtain a global map registration residual; and / or, a second acquisition subunit, used to select from the historical frame point cloud data continuous key frame point cloud data that is located before the current frame point cloud data and has target relevance to the current frame point cloud data; the static frame point cloud data is registered with the continuous 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 the historical point cloud data based on a loop detection algorithm to generate loop frame point cloud data; the static frame point cloud data is registered with the loop frame point cloud data to generate a loop detection registration residual.

[0091] The above device can execute a dynamic scene positioning 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 of executing a dynamic scene positioning method based on a known point cloud map. For technical details not fully described in this embodiment, please refer to a dynamic scene positioning method based on a known point cloud map provided in an embodiment of the present invention.

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

[0093] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the method according to various embodiments of the present application described in the above-mentioned "Exemplary Method" section of this specification.

[0094] The computer program product may be written in any combination of one or more programming languages ​​to write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0095] In addition, an embodiment of the present application may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the steps of the method according to the following embodiments of the present application described in the above "Exemplary Method" section of this specification.

[0096] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0097] The basic principles of the present application are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present application. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, not for limitation, and the above details do not limit the present application to being implemented by adopting the above specific details.

[0098] The block diagrams of the devices, apparatuses, equipment, and systems involved in this application are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagram. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open words, referring to "including but not limited to", and can be used interchangeably with them. The words "or" and "and" used here refer to the words "and / or" and can be used interchangeably with them, unless the context clearly indicates otherwise. The words "such as" used here refer to the phrase "such as but not limited to", and can be used interchangeably with them.

[0099] It should also be noted that in the apparatus, device and method of the present application, each component or each step can be decomposed and / or recombined. Such decomposition and / or recombination should be regarded as equivalent solutions of the present application.

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

[0101] The above description has been given for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although multiple example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

[0102] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification and the features of different embodiments or examples, unless they are contradictory.

[0103] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0104] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A dynamic scene positioning method based on a known point cloud map, characterized in that: include: 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; For any current frame point cloud data among the plurality of frames of point cloud data: based on the current moment, 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 observed posture of the robot, and output a current posture estimation value; The pose corresponding to the current frame point cloud data is adjusted based on the current pose estimation value to obtain the target frame point cloud data; the target frame point cloud data is subjected to dynamic point cloud filtering processing to 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, the robot's observation pose is globally optimized to output the current optimal observation pose.

2. The method according to claim 1, characterized in that: The method of estimating the observed posture of the robot based on the current frame inertial measurement data and the current frame point cloud data, and outputting a current posture estimation value, comprises: Obtaining a previous predicted pose corresponding to a previous frame of point cloud data adjacent to the current frame of point cloud data; Based on the last predicted posture and the current frame inertial measurement data, predict the observed posture of the robot at the current moment, and output the current predicted posture; Based on the difference between the observation value corresponding to the current frame point cloud data and the expected observation value corresponding to the current predicted posture, the current predicted posture is corrected to generate a current posture estimate.

3. The method according to claim 1, characterized in that The step of performing dynamic point cloud filtering on the target frame point cloud data and outputting static frame point cloud data comprises: Performing rasterization processing on the target frame point cloud data, and outputting a plurality of raster units of the same size; For any current grid cell among several grid cells: determine the number of points located in the current grid cell; determine the current height variance of the current grid cell 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 cell in the previous 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 previous height variance is greater than a first preset threshold and the number of points is greater than a second preset threshold, determine that the current grid cell is a dynamic grid; Each of the dynamic grids is deleted from the target frame point cloud data to obtain static frame point cloud data.

4. The method according to claim 3, characterized in that The determining the number of points located in the current grid cell comprises: Obtaining grid point cloud data corresponding to the current grid unit; For any target point in the grid point cloud data: obtaining the horizontal coordinate value and the vertical coordinate value of the target point; if both the horizontal coordinate value and the vertical coordinate value of the target point fall within the current grid cell, determining that the target point is located within the current grid cell, and determining the target point as a grid point; if at least one of the horizontal coordinate value and the vertical coordinate value of the target point does not fall within the current grid cell, determining that the target point is not located within the current grid cell; Based on the number of all grid points in the grid point cloud data, the number of points located in the current grid cell is determined.

5. The method according to claim 1, characterized in that The method of performing global optimization processing on the robot's observation posture 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 posture, comprises: Based on the static frame point cloud data and the 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 posture to obtain a constrained residual; When the constraint residual is the smallest, the current optimal observation pose is determined.

6. The method according to claim 5, characterized in that When the historical point cloud data is a SLAM map, the nonlinear optimization algorithm is used to perform global optimization processing on the robot observation posture based on the static frame point cloud data and the historical point cloud data of the target dynamic scene to obtain the constrained residual; including: Get the SLAM map of the target dynamic scene; The static frame point cloud data is aligned with the SLAM map based on a global map matching algorithm to obtain a global map registration residual.

7. The method according to claim 5, characterized in that When the historical point cloud data includes a SLAM map and historical frame point cloud data, the robot observation posture is globally optimized 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 posture; including: 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, selecting from the historical frame point cloud data continuous key frame point cloud data that is located before the current frame point cloud data and has target relevance to the current frame point cloud data; performing registration calculation on the static frame point cloud data and the continuous key frame point cloud data to generate a key frame registration residual; And / or, based on the loop detection algorithm, similarity recognition processing is performed on the current frame point cloud data and the historical point cloud data to generate loop frame point cloud data; the static frame point cloud data is aligned with the loop frame point cloud data to generate a loop detection alignment residual.

8. A dynamic scene positioning device based on a known point cloud map, characterized in that: include: The data acquisition module is used to acquire the 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; A posture optimization module is used for determining, for any current frame of point cloud data among the plurality of frames of point cloud data, current frame inertial measurement data corresponding to the current frame of point cloud data based on a current moment; estimating the observed posture of the robot based on the current frame inertial measurement data and the current frame of point cloud data, and outputting a current posture estimation value; The pose corresponding to the current frame point cloud data is adjusted based on the current pose estimation value to obtain the target frame point cloud data; the target frame point cloud data is subjected to dynamic point cloud filtering processing to 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, the robot's observation pose is globally optimized to output the current optimal observation pose.

9. An electronic device, comprising: processor; a memory for storing instructions executable by the processor; The processor is used to read the executable instructions from the memory and execute the instructions to implement the method according to any one of claims 1-7.

10. A computer readable medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the method according to any one of claims 1 to 7.

Images