A positioning method, device, robot and medium based on dynamic scenes

Abstract

The present invention provides a positioning method based on a dynamic scenario, which includes the following steps: S1, using the acquired 3D lidar data and performing positioning based on a prior map; S2, when the positioning result based on the prior map is incorrect, using a laser odometer to obtain the pose T of the robot m ; S3, adding the pose transformation obtained from the current frame and the previous frame of the laser odometer to the factor graph. There is a previous pose in the factor graph, and the initial pose value at the current moment is obtained through the predicted pose transformation of the laser odometer. This initial value is used as the initial value for the observed NDT registration, and the final result of the NDT registration is the accurate pose at the current moment. The present invention first uses the prior map of the lidar for pose matching. In the case of failed matching, the pose transformation between the current frame and the previous frame is obtained using the laser odometer, and the pose transformation obtained using the laser odometer is used as the initial value of the observation to obtain the final pose.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a positioning method, device, robot and medium based on dynamic scenes. Background Art

[0002] Existing outdoor laser SLAM positioning algorithms are all based on static environment assumptions, and most of them use registration algorithms as observation corrections to predict postures based on existing prior maps. However, when dynamic obstacles appear in the environment, including pedestrians, moving vehicles, etc., or when the prior map undergoes significant changes, it is difficult for the static registration algorithm to converge accurately, and the positioning accuracy will be greatly reduced.

[0003] Outdoor patrol robots usually work in large-scale scenarios such as parks, factories, and parking lots. For example, in a parking lot, a priori map is first established and navigation and other functions are enabled. Later, most vehicles disappear, causing the map to change. At this time, the positioning system based on the priori map will cause the positioning registration to diverge due to the change in the map and cannot converge to the most appropriate posture, greatly reducing the robustness of the positioning system.

[0004] The current solution to dynamic scenes is as follows: when positioning is lost or the scene changes greatly, the prior map is re-established. Re-establishing the prior map is relatively cumbersome.

[0005] The background description provided herein is for the purpose of generally presenting the context of the present disclosure. Unless otherwise indicated herein, the materials described in this section are not prior art to the claims of this application and are not admitted to be prior art by inclusion in this section. Summary of the invention

[0006] In view of the above technical problems in the related art, the present invention proposes 1. a positioning method based on a dynamic scene, which comprises the following steps:

[0007] S1, uses the acquired 3D lidar data and performs positioning based on the prior map;

[0008] S2, when the positioning result based on the prior map is incorrect, use the laser odometer to obtain the robot's posture T m ;

[0009] S3, add the posture transformation obtained by the laser odometer of the current frame and the previous frame to the factor graph. The factor graph contains the posture of the previous moment. The initial value of the posture at the current moment is obtained through the predicted posture transformation of the laser odometer. This initial value is used as the initial value of the observation NDT registration. The final result of the NDT registration is the current accurate posture.

[0010] Specifically, the positioning based on the prior map is based on NDT registration.

[0011] Specifically, the step S1 specifically includes: S11, dividing the prior map map into cubes;

[0012] S12, calculating the probability distribution model of each cube;

[0013] S13, transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point x′ i ;

[0014] S14, calculating the probability density of each conversion point falling in the corresponding cube according to the probability distribution model;

[0015] S15, the NDT registration score is obtained by adding the probability density calculated for each cube;

[0016] S16, optimize the score according to the Newton optimization algorithm to find the optimal posture so that the score value is maximized;

[0017] S17, determining whether the score exceeds a threshold value. If the score does not exceed the threshold value, the positioning result based on the priori map is incorrect.

[0018] Specifically, the step S2 specifically includes:

[0019] S21, obtaining a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame;

[0020] S22, obtaining a submap;

[0021] S23, align the current frame with the previous frame using the GICP algorithm to obtain the relative posture transformation T between the two frames s ;

[0022] S24, T s Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the coordinate system of the prior map

[0023] S25, using the posture prediction value The GICP algorithm is used to align the current frame with the submap to obtain the posture T of the current frame in the coordinate system of the prior map. m .

[0024] Specifically, the step S21 specifically includes:

[0025] Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture;

[0026] Otherwise, it is determined whether the current frame and the previous frame exceed the first threshold, or the angle has rotated beyond the first threshold. If so, the current frame is added to the key frame queue.

[0027] In a second aspect, another embodiment of the present invention discloses a positioning device based on a dynamic scene, which includes the following units:

[0028] A priori map positioning unit, used to use the acquired 3D lidar data and perform positioning based on the priori map;

[0029] The laser attitude calculation unit is used to obtain the robot's attitude T using the laser odometer when the positioning result based on the prior map is incorrect. m ;

[0030] The posture correction unit is used to add the posture transformation obtained by the laser odometer of the current frame and the previous frame into the factor graph. The factor graph contains the posture of the previous moment. The initial value of the posture at the current moment is obtained by transforming the predicted posture transformation of the laser odometer. This initial value is used as the initial value of the observed NDT registration. The final result of the NDT registration is the current accurate posture.

[0031] Specifically, the priori map positioning unit also includes:

[0032] A cube division unit is used to divide the prior map map into cubes;

[0033] A probability distribution model acquisition unit, used for calculating the probability distribution model of each cube;

[0034] The current frame conversion unit is used to transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point x′ i ;

[0035] A probability density calculation unit, used to calculate the probability density of each conversion point falling in the corresponding cube according to the probability distribution model;

[0036] NDT registration score calculation unit, used to obtain the NDT registration score by adding the probability density calculated for each cube;

[0037] NDT registration score optimization unit is used to optimize the score according to the Newton optimization algorithm and find the optimal posture to maximize the score value;

[0038] The priori map positioning result judgment unit is used to judge whether the score exceeds a threshold. If it does not exceed the threshold, the positioning result based on the priori map is incorrect.

[0039] Specifically, the laser posture calculation unit also includes:

[0040] A key frame acquisition unit, used for acquiring a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame;

[0041] A submap acquisition unit, used to acquire a submap;

[0042] The inter-frame posture acquisition unit is used to perform GICP algorithm registration on the current frame and the previous frame to obtain the relative posture transformation T between the two frames. s ;

[0043] The inter-frame posture prediction unit is used to transform T s Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the prior map coordinate system

[0044] Sub-image pose acquisition unit, used to use the pose prediction value The GICP algorithm is used to align the current frame with the submap to obtain the posture T of the current frame in the prior map coordinate system. m .

[0045] Specifically, the key frame acquisition unit also includes:

[0046] Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture;

[0047] Otherwise, it is determined whether the current frame and the previous frame exceed the first threshold, or the angle has rotated beyond the first threshold. If so, the current frame is added to the key frame queue.

[0048] In a third aspect, another embodiment of the present invention discloses a robot, comprising a central processing unit, a memory, a 3D laser radar, and a laser odometer, wherein the memory stores instructions, and the processor is used to implement the above-mentioned dynamic scene-based positioning method when executing the instructions.

[0049] In a fourth aspect, another embodiment of the present invention discloses a non-volatile storage medium, on which instructions are stored, and when the instructions are executed by a processor, they are used to implement a positioning method based on a dynamic scene.

[0050] The positioning method based on dynamic scenes of the present invention first uses the prior map of the laser radar for posture matching. In the case of matching failure, the laser odometer is used to obtain the posture of the current frame, and the posture obtained by the laser odometer is used to correct the posture obtained by the prior map to obtain the final posture. The present invention can solve the problem of inaccurate posture matching due to the changes of dynamic objects in dynamic scenes due to the prior map. The method of the present invention does not need to re-establish the prior map, and can avoid a large number of operations of re-establishing the map. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0052] Figure 1 is a flow chart of a positioning method based on a dynamic scene provided by an embodiment of the present invention;

[0053] Figure 2 is a schematic diagram of a positioning device based on a dynamic scene provided by an embodiment of the present invention;

[0054] Figure 3 It is a schematic diagram of a positioning device based on a dynamic scene provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0055] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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 ordinary technicians in this field belong to the scope of protection of the present invention.

[0056] Embodiment 1

[0057] The robot of this embodiment includes a 3D laser radar device, an IMU and a wheel odometer.

[0058] The outdoor scene of this embodiment has many cars, trees, buildings, etc. (generally, outdoor robots can easily guarantee this), and the positioning method based on prior map registration fails in the current scene.

[0059] refer to Figure 1 This embodiment discloses a positioning method based on a dynamic scene, which includes the following steps:

[0060] S1, uses the acquired 3D lidar data and performs positioning based on the prior map;

[0061] The robot of this embodiment has a central processor, and the central processor receives 3D laser radar data through a callback function. The 3D laser radar data includes but is not limited to: x-coordinate value, y-coordinate value, z-coordinate value, and timestamp of each point. The prior attitude is obtained by using IMU data and wheel odometer data, and NDT alignment is performed by using the prior attitude. The specific process is as follows:

[0062] S11, divide the prior map into cubes;

[0063] The prior map map of this embodiment is a map established in advance by the robot. After the robot establishes the map, the prior map may change. For example, some dynamic scenes such as vehicles staying in the prior map drive away, causing the prior map to change.

[0064] The size of the cube distribution in this embodiment is 20 cm.

[0065] S12, calculating the probability distribution model of each cube;

[0066] Calculate the mean q and covariance σ of the points contained in each cube separately:

[0067]

[0068]

[0069] where x i is the 3D coordinate of the i-th point in the current cube.

[0070] Then the probability model p(x) for each point in the cube is:

[0071]

[0072] where x is the 3D point in the current cube.

[0073] S13, transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point x′ i ;

[0074] S14, calculating the probability density of each conversion point falling in the corresponding cube according to the probability distribution model;

[0075] S15, the NDT registration score is obtained by adding the probability density calculated for each cube;

[0076]

[0077] S16, optimize the score according to the Newton optimization algorithm to find the optimal posture so that the score value is maximized;

[0078] S17, determining whether the score exceeds a threshold value. If the score does not exceed the threshold value, the positioning result based on the priori map is incorrect.

[0079] Finally, the score is used to determine whether the registration is successful. The current score is set to 2.0. At this time, in a dynamic scene, the score of the registration algorithm is difficult to reach the threshold. If the threshold is not reached, it means that the positioning effect is not good. At this time, the laser odometer is started.

[0080] S2, when the positioning result based on the prior map is incorrect, use the laser odometer to obtain the robot's posture T m .

[0081] The robot maintains a laser odometry factor in advance, uses the laser odometry to obtain the pose between two frames, and adds it to the entire positioning constraint.

[0082] The specific steps are as follows:

[0083] S21, obtaining a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame;

[0084] Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture;

[0085] Otherwise, it will determine whether the current frame is more than 30cm away from the previous frame, or the angle has rotated 45 degrees. If it exceeds the threshold, the current frame will be added to the key frame queue.

[0086] It is determined whether the number of key frames in the key frame queue exceeds 100 frames. If so, the key frames are discarded from the head of the queue to ensure that the number of key frames in the key frame queue is at most 100 frames.

[0087] Specifically, before step S21, the method further includes:

[0088] S20, downsampling each frame acquired by the laser radar;

[0089] The downsampling parameter in this embodiment is set to 0.5m.

[0090] S22, obtaining a submap;

[0091] Select the 50 frames closest to the current frame and form a submap based on the posture of each frame.

[0092] S23, align the current frame with the previous frame using the GICP algorithm to obtain the relative posture transformation T between the two frames s ;

[0093] S24, T s Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the prior map coordinate system

[0094] S25, using the posture prediction value The GICP algorithm is used to align the current frame with the submap to obtain the posture T of the current frame in the prior map coordinate system. m .

[0095] The calculation steps of the GICP algorithm of this embodiment are as follows:

[0096] S30, calculating corresponding points.

[0097] S31, calculate the covariance matrix of the 5 nearest neighbor points of each point in the source and target point clouds.

[0098] S32 is calculated as follows:

[0099]

[0100] In the above formula, d i is the Euclidean distance between corresponding points, is the covariance matrix of the i-th point in the target point cloud, is the covariance matrix of the i-th point in the source point cloud, minimizes the above objective function, and finds the optimal transformation T.

[0101] S3, add the posture transformation obtained by the laser odometer of the current frame and the previous frame to the factor graph. The factor graph contains the posture of the previous moment. The initial value of the posture at the current moment is obtained through the predicted posture transformation of the laser odometer. This initial value is used as the initial value of the observation NDT registration. The final result of the NDT registration is the current accurate posture.

[0102] The positioning method based on dynamic scenes of this embodiment first uses the laser radar's prior map for posture matching. In the case of matching failure, the laser odometer is used to obtain the posture of the current frame, and the posture obtained by the laser odometer is used to correct the posture obtained by the prior map to obtain the final posture. This embodiment can solve the problem of inaccurate posture matching due to the changes of dynamic objects in dynamic scenes due to the prior map. The method of this embodiment does not need to re-establish the prior map, which can avoid a large number of operations to re-establish the map.

[0103] Embodiment 2

[0104] refer to Figure 2 This embodiment discloses a positioning device based on a dynamic scene, which includes the following units:

[0105] A priori map positioning unit, used to use the acquired 3D lidar data and perform positioning based on the priori map;

[0106] The robot of this embodiment has a central processor, and the central processor receives 3D laser radar data through a callback function, and the 3D laser radar data includes but is not limited to: x-coordinate value, y-coordinate value, z-coordinate value, and timestamp of each point. The prior attitude is obtained by using IMU data and wheel odometer data, and NDT alignment is performed by using the prior attitude, which also includes the following units:

[0107] A cube division unit is used to divide the prior map map into cubes;

[0108] The prior map map of this embodiment is a map established in advance by the robot. After the robot establishes the map, the prior map may change. For example, some dynamic scenes such as vehicles staying in the prior map drive away, causing the prior map to change.

[0109] The size of the cube distribution in this embodiment is 20 cm.

[0110] A probability distribution model acquisition unit, used for calculating the probability distribution model of each cube;

[0111] Calculate the mean q and covariance σ of the points contained in each cube separately:

[0112]

[0113]

[0114] where x i is the 3D coordinate of the i-th point in the current cube.

[0115] Then the probability model p(x) for each point in the cube is:

[0116]

[0117] where x is the 3D point in the current cube.

[0118] The current frame conversion unit is used to transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point x′ i ;

[0119] A probability density calculation unit, used to calculate the probability density of each conversion point falling in the corresponding cube according to the probability distribution model;

[0120] NDT registration score calculation unit, used to obtain the NDT registration score by adding the probability density calculated for each cube;

[0121]

[0122] NDT registration score optimization unit is used to optimize the score according to the Newton optimization algorithm and find the optimal posture to maximize the score value;

[0123] The priori map positioning result judgment unit is used to judge whether the score exceeds a threshold. If it does not exceed the threshold, the positioning result based on the priori map is incorrect.

[0124] Finally, the score is used to determine whether the registration is successful. The current score is set to 2.0. At this time, in a dynamic scene, the score of the registration algorithm is difficult to reach the threshold. If the threshold is not reached, it means that the positioning effect is not good. At this time, the laser odometer is started.

[0125] The laser attitude calculation unit is used to obtain the robot's attitude T using the laser odometer when the positioning result based on the prior map is incorrect. m .

[0126] The robot maintains a laser odometry factor in advance, uses the laser odometry to obtain the pose between two frames, and adds it to the entire positioning constraint.

[0127] Also includes the following units:

[0128] A key frame acquisition unit, used for acquiring a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame;

[0129] Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture;

[0130] Otherwise, it will determine whether the current frame is more than 30cm away from the previous frame, or the angle has rotated 45 degrees. If it exceeds the threshold, the current frame will be added to the key frame queue.

[0131] It is determined whether the number of key frames in the key frame queue exceeds 100 frames. If so, the key frames are discarded from the head of the queue to ensure that the number of key frames in the key frame queue is at most 100 frames.

[0132] Specifically, it also includes:

[0133] A downsampling unit is used to downsample each frame from the laser;

[0134] The downsampling parameter in this embodiment is set to 0.5m.

[0135] A submap acquisition unit, used to acquire a submap;

[0136] Select the 50 frames closest to the current frame and form a submap based on the posture of each frame.

[0137] The inter-frame posture acquisition unit is used to perform GICP algorithm registration on the current frame and the previous frame to obtain the relative posture transformation T between the two frames. s ;

[0138] The inter-frame pose prediction unit is used to transform T s Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the prior map coordinate system

[0139] Sub-image pose acquisition unit, used to use the pose prediction value The GICP algorithm is used to align the current frame with the submap to obtain the posture T of the current frame in the prior map coordinate system. m .

[0140] The calculation steps of the GICP algorithm of this embodiment are as follows:

[0141] S30, calculating corresponding points.

[0142] S31, calculate the covariance matrix of the 5 nearest neighbors of each point in the source and target point clouds.

[0143] S32 is calculated as follows:

[0144]

[0145] In the above formula, d i is the Euclidean distance between corresponding points, is the covariance matrix of the i-th point in the target point cloud, is the covariance matrix of the i-th point in the source point cloud, minimizes the above objective function, and finds the optimal transformation T.

[0146] The posture correction unit is used to add the posture transformation obtained by the laser odometer of the current frame and the previous frame into the factor graph. The factor graph contains the posture of the previous moment. The initial value of the posture at the current moment is obtained by transforming the predicted posture transformation of the laser odometer. This initial value is used as the initial value of the observed NDT registration. The final result of the NDT registration is the current accurate posture.

[0147] The positioning method based on dynamic scenes of this embodiment first uses the laser radar's prior map for posture matching. In the case of matching failure, the laser odometer is used to obtain the posture of the current frame, and the posture obtained by the laser odometer is used to correct the posture obtained by the prior map to obtain the final posture. This embodiment can solve the problem of inaccurate posture matching due to the changes of dynamic objects in dynamic scenes due to the prior map. The method of this embodiment does not need to re-establish the prior map, which can avoid a large number of operations to re-establish the map.

[0148] Embodiment 3

[0149] This embodiment discloses a robot, which includes a central processing unit, a memory, a 3D laser radar, and a laser odometer. The memory stores instructions, and the processor is used to implement a positioning method based on dynamic scenes when executing the instructions.

[0150] Embodiment 4

[0151] refer to Figure 3 , Figure 3 : is a structural diagram of a positioning device based on a dynamic scene in this embodiment. The positioning device 20 based on a dynamic scene in this embodiment includes a processor 21, a memory 22, and a computer program stored in the memory 22 and executable on the processor 21. When the processor 21 executes the computer program, the steps in the above method embodiment are implemented. Alternatively, when the processor 21 executes the computer program, the functions of each module / unit in the above device embodiments are implemented.

[0152] Exemplarily, the computer program may be divided into one or more modules / units, which are stored in the memory 22 and executed by the processor 21 to complete the present invention. The one or more modules / units may be a series of computer program instruction segments capable of completing specific functions, which are used to describe the execution process of the computer program in the dynamic scene-based positioning device 20. For example, the computer program may be divided into the modules in Embodiment 2. For the specific functions of each module, please refer to the working process of the device described in the above embodiment, which will not be repeated here.

[0153] The positioning device 20 based on dynamic scenes may include, but is not limited to, a processor 21 and a memory 22. Those skilled in the art will appreciate that the schematic diagram is merely an example of the positioning device 20 based on dynamic scenes, and does not constitute a limitation on the positioning device 20 based on dynamic scenes, and may include more or fewer components than shown in the figure, or combine certain components, or different components, for example, the positioning device 20 based on dynamic scenes may also include input and output devices, network access devices, buses, etc.

[0154] The processor 21 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc. The processor 21 is the control center of the positioning device 20 based on dynamic scenes, and uses various interfaces and lines to connect various parts of the positioning device 20 based on dynamic scenes.

[0155] The memory 22 can be used to store the computer program and / or module. The processor 21 realizes various functions of the positioning device 20 based on dynamic scenes by running or executing the computer program and / or module stored in the memory 22 and calling the data stored in the memory 22. The memory 22 can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, an application required for at least one function (such as a sound playback function, an image playback function, etc.), etc.; the data storage area can store data created according to the use of the mobile phone (such as audio data, a phone book, etc.), etc. In addition, the memory 22 can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0156] Wherein, if the module / unit integrated in the positioning device 20 based on the dynamic scene is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor 21. Wherein, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium, etc. It should be noted that the content contained in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media does not include electrical carrier signals and telecommunication signals.

[0157] It should be noted that the device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, in the accompanying drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines. A person of ordinary skill in the art may understand and implement it without paying any creative effort.

[0158] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A positioning method based on dynamic scenes, comprising the following steps: S1, uses the acquired 3D lidar data and performs positioning based on the prior map; S2, when the positioning result based on the prior map is incorrect, use the laser odometer to obtain the robot's posture ; The step S2 specifically includes: S21, obtaining a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame; S22, obtaining a submap; S23, perform GICP algorithm registration on the current frame and the previous frame to obtain the relative posture change between the two frames ; S24, will Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the coordinate system of the prior map ; S25, using the posture prediction value , and perform the GICP algorithm to align the current frame with the submap to obtain the pose of the current frame in the coordinate system of the prior map ; S3, the relative posture transformation obtained by the laser odometer of the current frame and the previous frame is added to the factor graph. The factor graph contains the posture of the previous moment, so as to obtain the initial value of the posture at the current moment. This initial value is used as the initial value of the observation NDT registration. The final result of the NDT registration is the current accurate posture. 2 . The method according to claim 1 , wherein the positioning based on the a priori map is based on NDT registration.

3. According to the method of claim 2, the step S1 specifically comprises: S11, divide the prior map map into cubes; S12, calculating the probability distribution model of each cube; S13, transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point ; S14, calculating the probability density of each conversion point falling in the corresponding cube according to the probability distribution model; S15, the NDT registration score is obtained by adding the probability density calculated for each cube; S16, optimize the score according to the Newton optimization algorithm to find the optimal posture to maximize the score value; S17, determining whether the score exceeds a threshold value. If the score does not exceed the threshold value, the positioning result based on the priori map is incorrect.

4. According to the method of claim 3, the step S21 specifically comprises: Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture; Otherwise, it is determined whether the current frame and the previous frame exceed the first threshold, or the angle has rotated beyond the first threshold. If so, the current frame is added to the key frame queue.

5. A positioning device based on a dynamic scene, comprising the following units: A priori map positioning unit, used to use the acquired 3D lidar data and perform positioning based on the priori map; Laser attitude calculation unit, used to obtain the robot's attitude using a laser odometer when the positioning result based on the prior map is incorrect ; The laser posture calculation unit also includes: A key frame acquisition unit, used for acquiring a key frame, adding the key frame to a key frame queue, and saving the laser posture of the key frame; A submap acquisition unit, used to acquire a submap; The inter-frame posture acquisition unit is used to perform GICP algorithm registration on the current frame and the previous frame to obtain the relative posture change between the two frames. ; The inter-frame pose prediction unit is used to Multiply it by the posture of the previous frame to get the posture prediction value of the current frame in the coordinate system of the prior map ; Sub-image pose acquisition unit, used to use the pose prediction value , and perform the GICP algorithm to align the current frame with the submap to obtain the pose of the current frame in the coordinate system of the prior map ; The posture correction unit is used to transform the relative posture obtained by the laser odometer of the current frame and the previous frame Add it to the factor graph, which contains the posture of the previous moment, so as to obtain the initial value of the posture at the current moment, and use this initial value as the initial value of the observation NDT registration. The final result of NDT registration is the current accurate posture.

6. The device according to claim 5, wherein the a priori map positioning unit further comprises: A cube division unit is used to divide the prior map map into cubes; A probability distribution model acquisition unit, used for calculating the probability distribution model of each cube; The current frame conversion unit is used to transform the current frame obtained by the laser radar into the corresponding cube under the map according to the prior posture to obtain the corresponding conversion point ; A probability density calculation unit, used to calculate the probability density of each conversion point falling in the corresponding cube according to the probability distribution model; NDT registration score calculation unit, used to obtain the NDT registration score by adding the probability density calculated for each cube; NDT registration score optimization unit is used to optimize the score according to the Newton optimization algorithm and find the optimal posture to maximize the score value; The priori map positioning result judgment unit is used to judge whether the score exceeds a threshold. If it does not exceed the threshold, the positioning result based on the priori map is incorrect.

7. The device according to claim 6, wherein the key frame acquisition unit further comprises: Determine whether the current frame obtained by the laser odometer is the first frame. If it is the first frame, add the current frame to the key frame queue and save the current laser posture; Otherwise, it is determined whether the current frame and the previous frame exceed the first threshold, or the angle has rotated beyond the first threshold. If so, the current frame is added to the key frame queue.

8. A robot, comprising a central processing unit, a memory, a 3D laser radar, and a laser odometer, wherein the memory stores instructions, and the processor is used to implement the method described in any one of claims 1 to 4 when executing the instructions.

Images