Laser dynamic matching method, electronic device and storage medium

Abstract

The present application is about a method, electronic device and storage medium for laser dynamic matching, which is applied to a robot system, sets the initial state of the system, predicts the current state and covariance based on the initial state of the system; optimizes the state prediction of the system through a square root information filter based on the data of the encoder and the data of the inertial measurement sensor, and obtains the state transfer function and the covariance transfer function; matches the scanning result of the laser sensor with the map built offline in advance, obtains the rotation and displacement of the current positioning state; predicts the state estimation and predicts the covariance; constrains the search range and search direction of the laser matching through the predicted covariance, and gives the search range of the laser sensor. The laser dynamic matching method provides the laser sensor with a better initial posture and an observed search range, and dynamically adjusts the search range of the laser sensor during the search process to obtain better observation results.

Description

Technical Field

[0001] The present application relates to the field of robot positioning technology, and in particular to a laser dynamic matching method, electronic equipment and storage medium. Background Art

[0002] In the related technologies, robots are generally positioned using iterative nearest neighbor point technology (ICP) or robust ICP technology of M estimation or ICP technology based on point-line or point-surface matching. However, each of these technologies has different defects and cannot perform robot positioning well.

[0003] The iterative nearest neighbor technique is composed of two main processes. The first step is to find the nearest neighbor points in the current position to form a point-to-point match. The second step is to solve the rigid body transformation that minimizes the point-to-point matching distance, and then repeat the above two steps to obtain the optimal rigid body transformation. The iterative nearest neighbor technique requires multiple iterations to obtain the best results, and the initial posture has a great influence on the results and the number of iterations. In addition, when there are dynamic objects in the point cloud, the point cloud matching will not be able to obtain the correct value.

[0004] Robust ICP technology of M estimation. Due to the existence of point cloud noise and dynamic points, the use of standard ICP technology often results in point cloud matching failure or inability to obtain high-precision matching. M estimation of the distance of the point cloud can greatly reduce the impact of outliers.

[0005] Based on point-line and point-surface matching ICP technology, the standard ICP technology uses point-point distance. In the point cloud, there are often a large number of geometric structures such as straight lines or planes. These geometric structures can accelerate the matching of point clouds and make the matching results more robust. Therefore, using point-line distance or point-surface distance can improve the performance of the ICP algorithm.

[0006] Due to the limitation of robot computing power, it is difficult to calculate a large number of laser ICP iterative matching in real time. Summary of the invention

[0007] In order to overcome the problems existing in the related art, the present application provides a laser dynamic matching method, an electronic device and a storage medium. The laser dynamic matching method uses the robot posture variance estimation obtained by data fusion of a laser sensor, a wheel encoder, and an inertial measurement sensor to provide the laser ICP with a better initial posture and an observation search range. At the same time, the laser search range is dynamically adjusted during the search process to obtain better observation results.

[0008] The first aspect of the present application provides a laser dynamic matching method, which is applied to a robot system and includes the following steps: Step 1, setting the initial state of the robot system, the initial state includes: the position of the system and the speed of the system; Step 2, predicting the current system state of the robot according to the initial state of the robot system, and obtaining the current state estimate and covariance of the robot system; Step 3, optimizing the current state estimate and covariance of the robot system through a square root information filter according to the data of the encoder and the data of the inertial measurement sensor, and obtaining a state transfer function and a covariance transfer function; Step 4, matching the scanning result of the laser sensor with a map previously built offline, and obtaining the rotation and displacement of the current positioning state; Step 5, obtaining a predicted state estimate and a predicted covariance according to the state transfer function and the covariance transfer function as well as the rotation and displacement of the current positioning state; Step 6, constraining the search range and search direction of the laser matching by the predicted covariance, and giving the search range of the laser sensor.

[0009] Preferably, the method further comprises: step 7, updating the search range of the laser sensor according to the optimal estimate of the next system state of the system.

[0010] Preferably, the initial state of the system is:

[0011] x0={v0,p0};

[0012] P0=R0 -1 R0 -T ;

[0013] Among them, x0 represents the true value of the system state at the initial moment, P0 is the initial covariance matrix of the system state, R0 is the square root of the initial information matrix of the system state, and v0 is the initial velocity of the system state.

[0014] Preferably, the state transfer function and the covariance transfer function are:

[0015]

[0016]

[0017] Δx={ΔR,Δp};

[0018] Where Φ and G are system parameters, P i is the covariance matrix of the system at time i, P a is the variance of the accelerometer noise, and represents the estimation of the parameter, Δx is the relative state transition between time i and time i-1, ΔR is the relative rotation, and Δp is the relative displacement.

[0019] Preferably, the calculation functions of Φ and G are:

[0020]

[0021]

[0022] Preferably, the matching function between the scanning result of the laser sensor and the map built offline in advance is:

[0023]

[0024] Among them, d i Refers to the distance value measured by the i-th laser beam, x i Refers to the coordinate value of the i-th laser beam in the system coordinate system, R j is the rotation of the system in the map coordinate system, p j is the translation of the system in the map coordinate system, M is the error map, and the error of the i-th laser beam is calculated.

[0025] Preferably, the function of the search range and search direction of the laser matching constrained by the predicted covariance is:

[0026] C x_i =||R i x ～ ||;

[0027]

[0028] d k (x i )=π(l k ,x i );

[0029]

[0030] where x i is the current state value required, C x_i It is the information of the current state after the state transfer. is the information contained in the current frame of laser observation, is the information sum of all laser lines, function π is the distance value that transfers the laser observation line from the current state value to the map, R i is the square root of the information matrix of the predicted state.

[0031] Preferably, the function of observing updates is:

[0032]

[0033]

[0034] A second aspect of the present application provides an electronic device, comprising: a memory; one or more processors; one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any of the methods.

[0035] The third aspect of the present application provides a storage medium storing a computer program, wherein when the computer program is executed by a processor, the method for laser dynamic matching is implemented.

[0036] The present application provides a laser dynamic matching method, electronic device and storage medium. The laser dynamic matching method uses the robot posture variance estimation obtained by data fusion of laser sensors, wheel encoders and inertial measurement sensors to provide laser ICP with a better initial posture and observation search range. At the same time, the laser search range is dynamically adjusted during the search process to obtain better observation results. At the same time, the same results obtained by traditional ICP technology can be obtained in a shorter time.

[0037] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The above and other objects, features and advantages of the present application will become more apparent through a more detailed description of exemplary embodiments of the present application in conjunction with the accompanying drawings, wherein the same reference numerals generally represent the same components in the exemplary embodiments of the present application.

[0039] Figure 1 It is a schematic flow chart of a laser dynamic matching method shown in an embodiment of the present application;

[0040] Figure 2 It is a schematic diagram of a laser sensor scanning the surrounding environment in a laser dynamic matching method shown in an embodiment of the present application;

[0041] Figure 3 is a schematic diagram of a robot scanning using a laser dynamic matching method shown in an embodiment of the present application;

[0042] Figure 4 It is the result of 1, 3, and 6 iterations of a laser dynamic matching method shown in an embodiment of the present application;

[0043] Figure 5 It is the result of 3, 8, and 15 iterations of the conventional laser ICP technology in the prior art;

[0044] Figure 6 It is a schematic diagram of the structure of an electronic device shown in an embodiment of the present application. DETAILED DESCRIPTION

[0045] The preferred embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to make the present application more thorough and complete, and to fully convey the scope of the present application to those skilled in the art.

[0046] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0047] It should be understood that although the terms "first", "second", "third", etc. may be used in this application to describe various information, this information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of this application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of this application, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0048] In response to the above problems, the present application provides a laser dynamic matching method, electronic device and storage medium. The laser dynamic matching method uses the robot posture variance estimation obtained by data fusion of laser sensors, wheel encoders, and inertial measurement sensors to provide the laser ICP with a better initial posture and observation search range, and dynamically adjusts the laser search range during the search process to obtain better observation results.

[0049] The technical solution of the embodiments of the present application is described in detail below with reference to the accompanying drawings.

[0050] Figure 1 The following is a flow chart of a laser dynamic matching method shown in an embodiment of the present application. A laser dynamic matching method is provided, which is applied to a robot system and includes the following steps:

[0051] Step 1: Set the initial state x0 of the robot system. The initial state includes the position P0 of the system and the speed v0 of the system. Specifically, the initial state of the system is:

[0052] x0={v0,p0};

[0053] P0=R0 -1 R0 -T ;

[0054] Among them, x0 represents the true value of the system state at the initial moment, P0 is the initial covariance matrix of the system state, R0 is the square root of the initial information matrix of the system state, and v0 is the initial velocity of the system state.

[0055] Step 2: predict the current system state of the robot based on the initial state of the robot system and obtain the current state x of the robot system i , that is, the state at time i, and the covariance P i , that is, the covariance at time i.

[0056] Step 3: Based on the encoder data and the inertial measurement sensor data, the current state of the robot system is estimated by a square root information filter. i The state transfer function and the covariance transfer function are optimized. Specifically, the state transfer function and the covariance transfer function are:

[0057]

[0058]

[0059] Δx={ΔR,Δp};

[0060] Where Φ and G are system parameters, P i is the covariance matrix of the system at time i, P a is the variance of the accelerometer noise, and It represents the estimation of the parameter, Δx is the relative state transition between time i and time i-1, where ΔR is the relative rotation and Δp is the relative displacement. The relative state transition Δx is calculated from the position data of the wheel encoder and the angle data of the inertial measurement sensor. In addition, the inertial measurement sensor obtains the relative rotation ΔR and the wheel encoder obtains the relative displacement Δp.

[0061] Specifically, the calculation functions of Φ and G are:

[0062]

[0063]

[0064] Step 4: By matching the scanning result of the laser sensor with the map built offline in advance, the rotation amount R of the current positioning state is obtained. j and displacement p jSpecifically, the matching function between the scanning result of the laser sensor and the map built offline in advance is:

[0065]

[0066] Among them, d i Refers to the distance value measured by the i-th laser beam, x i Refers to the coordinate value of the i-th laser beam in the system coordinate system, R j is the rotation of the system in the map coordinate system, p j is the translation of the system in the map coordinate system, M is the error map, and the error of the i-th laser beam is calculated. The above equation is a nonlinear equation, and the LM method is used for nonlinear optimization. However, solving nonlinear optimization equations requires a better initial solution. Inertial measurement sensors and wheel encoders provide a better initial solution for laser matching, and then the final solution is obtained faster, reducing the amount of calculation. The initial solution is: according to the state x at time i-1 i-1 Add the state transition amount Δx, and the state x at the i-th moment is obtained i The predicted value of .

[0067] Figure 2 This is a schematic diagram of a laser sensor scanning the surrounding environment in a laser dynamic matching method shown in an embodiment of the present application. The middle box diagram is a robot, and the grayscale shadows around it are a scan of the surrounding environment by the laser sensor. The shorter the line, the shorter the distance. In some embodiments, a 2D single-line laser is used, and the laser sensor rotates around the rotation axis and samples at equal intervals within a range of 360 degrees, usually scanning once every 1 / 3 degree or 1 / 4 degree. By matching the scanning results of the laser sensor with the map built offline in advance, the current positioning state rotation amount R can be obtained. j and displacement p j .

[0068] Step 5: Get the predicted state estimate based on the state transfer function and covariance transfer function as well as the rotation and displacement of the current positioning state and the predicted covariance

[0069] Step 6, through the predicted covariance Constrain the search range and search direction of laser matching, given the search range of the laser sensor. Specifically, the function of the search range and search direction of laser matching constrained by the predicted covariance is:

[0070] C x_i =||R i x ～ ||;

[0071]

[0072] d k (x i )=π(l k ,x i );

[0073]

[0074] where x i is the current state value required, C x_i It is the information of the current state after the state transfer. is the information contained in the current frame of laser observation, is the information sum of all laser lines, function π is the distance value that transfers the laser observation line from the current state value to the map, R i is the square root of the information matrix of the predicted state.

[0075] Figure 3 This is a schematic diagram of a robot scanning using a method of laser dynamic matching shown in an embodiment of the present application. The outer circle surrounding the robot represents the covariance of the robot's current error, which is obtained by modeling the inertial measurement sensor and encoder. The covariance is used to constrain the search position and search direction of the laser matching, further improving the speed of laser matching. In some embodiments, the laser matching algorithm is a nonlinear equation that is highly dependent on the initial solution. The initial solution of the laser matching of the laser sensor is provided by fusing the inertial measurement sensor and the encoder. At the same time, by modeling the errors of the inertial measurement sensor and the encoder, a matching solution direction is provided for the laser matching, further improving the speed of laser matching.

[0076] Step 7: Update the search range of the laser sensor based on the optimal estimate of the next system state. Specifically, the function of observation update is:

[0077]

[0078]

[0079] Due to R i The matrix is ​​originally an upper triangular matrix, so using Givens Rotation for QR decomposition can quickly upper triangularize the above matrix and obtain the optimal solution that satisfies the least squares.

[0080] Figure 4 is the result of 1, 3, and 6 iterations of a laser dynamic matching method shown in an embodiment of the present application. Figure 5 These are the results of 3, 8, and 15 iterations of the conventional laser ICP technology in the prior art. It can be seen that the present method can obtain the same result as the conventional ICP technology in a shorter time.

[0081] A second aspect of the present application provides an electronic device, comprising: a memory; one or more processors; one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any of the methods.

[0082] The third aspect of the present application provides a storage medium storing a computer program, wherein when the computer program is executed by a processor, the method for laser dynamic matching is implemented.

[0083] The present application provides a laser dynamic matching method, electronic device and storage medium. The laser dynamic matching method uses the robot posture variance estimation obtained by data fusion of laser sensors, wheel encoders and inertial measurement sensors to provide laser ICP with a better initial posture and observation search range. At the same time, the laser search range is dynamically adjusted during the search process to obtain better observation results. At the same time, the same results obtained by traditional ICP technology can be obtained in a shorter time.

[0084] Figure 6 It is a schematic diagram of the structure of an electronic device shown in an embodiment of the present application.

[0085] See also Figure 6 , the electronic device 1000 includes a memory 1010 and a processor 1020 .

[0086] The processor 1020 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 the processor may be any conventional processor, etc.

[0087] The memory 1010 may include various types of storage units, such as system memory, read-only memory (ROM), and permanent storage devices. Among them, ROM can store static data or instructions required by the processor 1020 or other modules of the computer. The permanent storage device may be a readable and writable storage device. The permanent storage device may be a non-volatile storage device that does not lose the stored instructions and data even after the computer is powered off. In some embodiments, the permanent storage device uses a large-capacity storage device (such as a magnetic or optical disk, flash memory) as a permanent storage device. In some other embodiments, the permanent storage device may be a removable storage device (such as a floppy disk, optical drive). The system memory may be a readable and writable storage device or a volatile readable and writable storage device, such as a dynamic random access memory. The system memory may store some or all instructions and data required by the processor at run time. In addition, the memory 1010 may include any combination of computer-readable storage media, including various types of semiconductor memory chips (DRAM, SRAM, SDRAM, flash memory, programmable read-only memory), and disks and / or optical disks may also be used. In some embodiments, the memory 1010 may include a readable and / or writable removable storage device, such as a laser disc (CD), a read-only digital versatile disc (such as a DVD-ROM, a double-layer DVD-ROM), a read-only Blu-ray disc, an ultra-density optical disc, a flash memory card (such as an SD card, a mini SD card, a Micro-SD card, etc.), a magnetic floppy disk, etc. The computer-readable storage medium does not include carrier waves and transient electronic signals transmitted wirelessly or wired.

[0088] The memory 1010 stores executable codes, and when the executable codes are processed by the processor 1020 , the processor 1020 can execute part or all of the methods described above.

[0089] The scheme of the present application has been described in detail above with reference to the accompanying drawings. In the above embodiments, the description of each embodiment has its own emphasis. For the part that is not described in detail in a certain embodiment, refer to the relevant description of other embodiments. Those skilled in the art should also know that the actions and modules involved in the description are not necessarily required for the present application. In addition, it can be understood that the steps in the method of the embodiment of the present application can be adjusted in order, merged and deleted according to actual needs, and the modules in the device of the embodiment of the present application can be merged, divided and deleted according to actual needs.

[0090] In addition, the method according to the present application may also be implemented as a computer program or a computer program product, which includes computer program code instructions for executing some or all of the steps in the above method of the present application.

[0091] Alternatively, the present application can also be implemented as a non-temporary machine-readable storage medium (or computer-readable storage medium, or machine-readable storage medium) on which executable code (or computer program, or computer instruction code) is stored. When the executable code (or computer program, or computer instruction code) is executed by a processor of an electronic device (or electronic device, server, etc.), the processor executes part or all of the steps of the above-mentioned method according to the present application.

[0092] Those skilled in the art will further appreciate that the various illustrative logical blocks, modules, circuits, and algorithm steps described in connection with the application herein may be implemented as electronic hardware, computer software, or combinations of both.

[0093] The flow chart and block diagram in the accompanying drawings show the possible architecture, function and operation of the system and method according to multiple embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a part of a module, a program segment or a code, and the part of the module, the program segment or the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two continuous square boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or the flow chart, and the combination of the square boxes in the block diagram and / or the flow chart can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0094] The embodiments of the present application have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or improvements to the technology in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A laser dynamic matching method, applied to a robot system, characterized in that: The following steps are involved: Step 1, setting the initial state of the robot system, the initial state includes: the position of the system and the speed of the system; Step 2, predicting the current system state of the robot according to the initial state of the robot system, and obtaining the current state estimation and covariance of the robot system; Step 3, according to the data of the encoder and the data of the inertial measurement sensor, the current state estimation and covariance of the robot system are optimized through a square root information filter to obtain a state transfer function and a covariance transfer function; Step 4, by matching the scanning result of the laser sensor with the map built offline in advance, the rotation amount and displacement amount of the current positioning state are obtained; Step 5, obtaining a predicted state estimate and a predicted covariance according to the state transfer function and the covariance transfer function as well as the rotation and displacement of the current positioning state; Step 6, constrain the search range and search direction of laser matching by the predicted covariance, given the search range of the laser sensor.

2. The laser dynamic matching method according to claim 1, characterized in that: Also includes: Step 7: Update the search range of the laser sensor based on the optimal estimate of the next system state of the system.

3. The laser dynamic matching method according to claim 2, characterized in that: The initial state of the system is: x0={v0,p0}; P0=R0 -1 R0 -T ; Among them, x0 represents the true value of the system state at the initial moment, P0 is the initial covariance matrix of the system state, R0 is the square root of the initial information matrix of the system state, and v0 is the initial velocity of the system state.

4. The laser dynamic matching method according to claim 3, characterized in that: The state transfer function and covariance transfer function are: Δx={ΔR,Δp}; Where Φ and G are system parameters, P i is the covariance matrix of the system at time i, P a is the variance of the accelerometer noise, and represents the estimation of the parameter, Δx is the relative state transition between time i and time i-1, where ΔR is the relative rotation, Δp is the relative displacement, and x i-1 Represents the state at time i-1.

5. The laser dynamic matching method according to claim 4, characterized in that: The calculation functions of Φ and G are:

6. The laser dynamic matching method according to claim 5, characterized in that: The matching function between the scanning result of the laser sensor and the map built offline in advance is: Among them, d i Refers to the distance value measured by the i-th laser beam, x i Refers to the coordinate value of the i-th laser beam in the system coordinate system, R j is the rotation of the system in the map coordinate system, p j is the translation of the system in the map coordinate system, M is the error map, and the error of the i-th laser beam is calculated.

7. An electronic device, characterized in that: include: Memory; one or more processors; One or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs comprising instructions for executing any one of the methods according to claims 1-6.

8. A storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for laser dynamic matching according to any one of claims 1 to 6 is implemented.

Images