Method and apparatus for positioning and mapping based on lidar and magnetic sensor

Abstract

The present invention discloses a positioning and mapping method and device based on a lidar and a magnetic sensor, which includes: Step 1, arranging magnetic nails, obtaining the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, and the pose transformation matrix T<supgt;m< / supgt> of the origin of the m-th frame of magnetic sensor relative to the first magnetic nail; Step 2, obtaining the current n-th frame of lidar point cloud p<subgt;n< / subgt>, p<subgt;1< / subgt> and the pose transformation matrix of p<subgt;n< / subgt> according to the change of the poses of two adjacent frames of lidar point clouds collected by the lidar; Step 3, calibrating the pose of the magnetic sensor relative to the lidar, and synchronizing the prior coordinates of the first magnetic nail and the m-th magnetic nail as the global coordinates Mag<subgt;1< / subgt>, Mag<subgt;m< / subgt> in the lidar coordinate system respectively; Step 4, calculating the velocity V<subgt;m< / subgt> of the magnetic sensor passing through the m-th magnetic nail, further optimizing the pose estimation of the lidar, and constructing a local map containing the waypoint Mag<subgt;m< / subgt> every preset number of frames; Step 5, recording each local map, performing loop detection, and generating a global map.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial intelligent robot positioning, and particularly to a positioning and mapping method and device based on a lidar and a magnetic sensor. Background Art

[0002] Currently, lidar positioning and magnetic sensor positioning are two common positioning methods for industrial intelligent robots. Among them, the lidar can realize the positioning and mapping functions of intelligent robots, and the magnetic sensor can provide high-precision positioning information under single-degree-of-freedom motion. Currently, the working environment of most industrial intelligent robots is a large workshop. The measurement range of the lidar is limited and it cannot be normally positioned in an open scene with low structure. Magnetic sensor positioning is widely used in the industrial field, but the workload of installing magnetic nails and magnetic strips for single magnetic sensor positioning is large and difficult to maintain. Summary of the Invention

[0003] The purpose of the present invention is to provide a positioning and mapping method and device based on a lidar and a magnetic sensor, which can fuse the lidar and the magnetic sensor for the positioning and mapping of industrial intelligent robots.

[0004] To achieve the above purpose, the present invention provides a positioning and mapping method based on a lidar and a magnetic sensor, which includes a mapping step and a positioning step;

[0005] Among them, the mapping step specifically includes:

[0006] Step 1, arrange magnetic nails in the working area of the selected unstructured scene, obtain the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail is and the pose of the m-th magnetic nail relative to the origin of the magnetic sensor Furthermore, according to Equation (1), obtain the pose transformation matrix T of the origin of the m-th frame magnetic sensor M relative to the first magnetic nail m , where the prior coordinates of the first magnetic nail are (0, 0), the prior coordinates of the m-th magnetic nail are determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, m≠1;

[0007]

[0008] Step 2, according to the change in the pose of two adjacent frames of lidar point clouds collected by the lidar L, obtain the current n-th frame lidar point cloud p n , p1 and p n pose transformation matrix T1 of n , where n = m + 1;

[0009] Step 3, calibrate the pose of the magnetic sensor M relative to the lidar L Obtained according to Step 1 T1 obtained in Step 2 n and p n , synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m ;

[0010] Step 4, according to T obtained in Step 1 m 、p n and the time difference Δt = t n-1 - t n between p n-1 , calculate the velocity V of the magnetic sensor M passing through the m-th magnetic nail m , further optimize the pose estimation of the lidar L, and construct a local map containing the waypoint Mag m at every preset number of frames:

[0011] Step 5, record each local map, perform loop detection, and generate a global map

[0012] Furthermore, in Step 2, the method for obtaining the pose transformation matrix T1 between p1 and p n specifically includes: n According to the relative translation distance and relative rotation angle in the x and y directions of the poses of two adjacent frames of lidar point clouds collected by the lidar L in the global coordinate system, obtain them using the following equations (2) and (3). First, obtain the pose transformation matrix

[0013] between p n and p n of the previous frame of lidar point cloud p n-1 ) according to equation (2), and then obtain the pose transformation matrix T1 between p1 and p n-1 using equation (3): n : n In equations (2) and (3),

[0014]

[0015]

[0016] is the pose transformation matrix between p and p n of the previous frame of lidar point cloud p n , n-1 is the pose transformation matrix from the (n - 2)-th frame of lidar point cloud p to p n-2 , R, t n-1 , θ, x n-1 , y n-1 are respectively the pose transformation matrix between p n-1 and p n-1to p n The rotation transformation matrix, translation transformation matrix, rotation angle, x-direction translation distance, and y-direction translation distance.

[0017] Furthermore, according to formula (4), the current n-th frame laser point cloud p is obtained n :

[0018] p n =T1 n p1 (4).

[0019] 4. The positioning and mapping method based on laser radar and magnetic sensor according to claim 1, characterized in that in step 3, the T1 obtained in step 2 n and p n , using formula (5), the prior coordinates of the first magnetic nail and the mth magnetic nail are synchronized to the global coordinates Mag1 and Mag2 in the laser radar coordinate system respectively. m ;

[0020]

[0021] Furthermore, in step 4, the speed V of the magnetic sensor M passing the mth magnetic nail is calculated using formula (6): m , further optimize the pose estimation of the laser radar L, and construct a map containing landmark points Mag every preset number of frames m Local map of:

[0022] V m =(T m -T m-1 ) / Δt (6)

[0023] In formula (6), T m-1 is the position change matrix of the origin of the magnetic sensor M relative to the first magnetic pin in the m-1th frame.

[0024] Furthermore, in step 4, the method for further optimizing the laser radar pose estimation specifically includes:

[0025] According to the T obtained in step 1 m , p obtained in step 2 n Mag obtained in step 3 m In the case of formula (9), p n Updated to Mag m , and Mag m as waypoints;

[0026]

[0027] Where D m For pn The absolute distance from p n-1 is C, where C is a constant.

[0028] The present invention also provides a positioning and mapping device based on a lidar and a magnetic sensor, which includes a mapping unit and a positioning unit;

[0029] Among them, the mapping unit specifically includes:

[0030] A magnetic nail related information acquisition sub-unit, which is used to arrange magnetic nails on the working site in a selected unstructured scene, obtain the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, and the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail is and the pose of the m-th magnetic nail relative to the origin of the magnetic sensor Furthermore, according to formula (1), obtain the pose transformation matrix T of the origin of the m-th frame magnetic sensor M relative to the first magnetic nail m , where the prior coordinates of the first magnetic nail are (0, 0), and the prior coordinates of the m-th magnetic nail are determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, m≠1;

[0031]

[0032] A pose solution operator unit, which is used to obtain the current n-th frame lidar point cloud p n , p1 and p n of the pose transformation matrix T1 n , where n = m + 1;

[0033] A coordinate system synchronization sub-unit, which is used to calibrate the pose of the magnetic sensor M relative to the lidar L According to what is obtained in step 1 T1 obtained in step 2 n and p n , synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m ;

[0034] A local map construction sub-unit, according to T obtained in step 1 m , p n and p n-1 of the time difference Δt = t n -t n-1 , calculate the speed V of the magnetic sensor M passing through the m-th magnetic nail m , further optimize the pose estimation of the lidar L, and construct a local map containing the waypoint Mag m every preset number of frames:

[0035] A global map generation unit, which is used to record each local map, perform loop detection, and generate a global map.

[0036] Further, the pose solution operator unit specifically includes:

[0037] A pose transformation matrix module, which is used to obtain the relative translation distance and relative rotation angle in the x and y directions in the global coordinate system according to the poses of two adjacent frames of lidar point clouds collected by the lidar L, and is obtained by the following formulas (2) and (3). First, obtain p according to formula (2) n and p n of the previous frame of lidar point cloud p n-1 pose transformation matrix Then obtain the pose transformation matrix T1 of p1 and p n according to formula (3) n :

[0038]

[0039]

[0040] In formulas (2) and (3), is the pose transformation matrix of p n and p n of the previous frame of lidar point cloud p n-1 , is the pose transformation matrix from the (n - 2)-th frame of lidar point cloud p n-2 to p n-1 , R, t n-1 , θ, x n-1 , y n-1 are respectively the rotation transformation matrix, translation transformation matrix, rotation angle, translation distance in the x direction, and translation distance in the y direction from p n-1 to p n ;

[0041] A lidar point cloud estimation module, which is used to obtain the current n-th frame of lidar point cloud p n according to formula (4):

[0042] p n = T1 n p1 (4).

[0043] Further, the local map construction subunit is specifically used to calculate the velocity V of the magnetic sensor M passing through the m-th magnetic nail by using formula (6) m , further optimize the pose estimation of the lidar L, and construct a local map containing the road sign Mag m every preset number of frames:

[0044] V m = (Tm -T m-1 ) / Δt (6)

[0045] In Equation (6), T m-1 is the pose change matrix of the origin of the m-1th frame magnetic sensor M relative to the first magnetic nail.

[0046] Furthermore, the method for further optimizing the pose estimation of the lidar by the local map construction subunit specifically includes:

[0047] According to the T obtained by the subunit for obtaining magnetic nail related information m , when p obtained by the pose solution operator subunit n and Mag obtained by the coordinate system synchronization subunit m meet the condition of Equation (9), update p n to Mag m , and use Mag m as a landmark;

[0048]

[0049] In the formula, D m is the absolute distance between p n and p n-1 , and C is a constant.

[0050] The magnetic nail positioning method in the present invention only needs to install a row of sparse magnetic nails on the driving track. The magnetic nails can be installed in scenes with large space and low structure and a certain distance in front of the parking point, without the need for two rows of continuous arrangements. Using a magnetic scale encoder and a magnetic nail detection sensor, measure the relative position information of the industrial intelligent robot and calculate the current speed. Update the position and pose of the robot according to the relative coordinates of each magnetic nail and the first magnetic nail. Use a high-precision total station to arrange the positions of the magnetic nails. During the positioning and mapping process, the positions of the magnetic nails provide landmarks and accurate initial values for lidar positioning. The Hall magnetic encoder in the magnetic scale can provide relative position information with an accuracy of 1 mm, making up for the positioning loss caused by the maximum working distance limitation of the lidar in the large space and low structure scene, and at the same time avoiding the calibration error and the problem of consuming a large amount of computing resources caused by excessive sensor fusion in Technique 1, thereby reducing the use of lidar. Brief Description of the Drawings

[0051] Figure 1 is the flowchart of the mapping function provided by the embodiment of the present invention.

[0052] Figure 2 is the flowchart of the positioning function provided by the embodiment of the present invention. Detailed Embodiments

[0053] In the accompanying drawings, the same or similar reference numerals are used to denote the same or similar elements or elements having the same or similar functions. The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0054] In the description of the present invention, the orientation or positional relationship indicated by the terms "center", "longitudinal", "lateral", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the accompanying drawings. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as limiting the protection scope of the present invention.

[0055] The positioning and mapping method based on lidar and magnetic sensor provided by the embodiment of the present invention includes a mapping step and a positioning step.

[0056] Among them, as Figure 1 shown, the mapping step specifically includes:

[0057] Step 1, first arrange magnetic nails on the working site in the selected unstructured scenario.

[0058] The arrangement method of the magnetic nails can be, for example, according to the working environment of the industrial intelligent robot, especially the unstructured scenario, such as: the scenario a certain distance in front of the parking point, or the open scenarios such as corridors and open spaces. Arrange magnetic nails on the driving track of the industrial intelligent robot. These magnetic nails do not need to be arranged continuously at close range. Their function is to assist lidar positioning and make up for the positioning loss phenomenon of lidar in unstructured scenarios. Among them, the unstructured environment has uneven surface material properties, irregular and unstable structure and size changes, and the environmental information is non-fixed, unknown, and indescribable.

[0059] The magnetic nail arrangement device can be to arrange the positions of magnetic nails using a total station with a preset accuracy (for example, within 2 mm) according to the working site in the selected unstructured scenario.

[0060] Then, install a magnetic scale sensor (hereinafter referred to as: magnetic sensor M) that integrates a magnetic encoder and a magnetic nail detection sensor on the industrial intelligent robot. Record the coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system through the magnetic sensor M. This coordinate is called the prior coordinate in the text. Then, the magnetic sensor on the industrial intelligent robot will provide continuous prior coordinate information of the magnetic nails when passing through the magnetic nails. Among them, the prior coordinate of the first magnetic nail is (0, 0), and the prior coordinate of the m-th magnetic nail is determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, m≠1.

[0061] At the same time, obtain the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail through the total station as and the pose T1 of the m-th magnetic nail relative to the origin of the magnetic sensor M , and then obtain the pose transformation matrix T of the origin of the magnetic sensor M relative to the m-th magnetic nail according to Equation (1) m . Among them, the origin of the magnetic sensor can be regarded as the first signal point of the magnetic sensor

[0062]

[0063] Step 2, pose calculation: Obtain the current n-th frame of lidar point cloud p according to the change in the pose of two adjacent frames of lidar point clouds collected by the lidar L n , p1 and p n 's pose transformation matrix T1 n , where n = m + 1. The lidar L is preferably implemented using a single-line lidar with a lower cost

[0064] In one embodiment, the method for obtaining the pose transformation matrix T1 of p1 and p n can be calculated using the IMLS-ICP method, that is: According to the relative translation distance and relative rotation angle in the x and y directions of the poses of two adjacent frames of lidar point clouds collected by the lidar L in the global coordinate system, obtain using the following Equations (2) and (3). First, obtain the pose transformation matrix of the previous frame of lidar point cloud p of p n and p n and p n using Equation (2), and then obtain the pose transformation matrix T1 of p1 and p n-1 using Equation (3) : n The pose transformation matrix T1 of p1 and p n :

[0065]

[0066]

[0067] In Equations (2) and (3), is the pose transformation matrix of the previous frame of lidar point cloud p of p n and p n and p n-1 , is the pose transformation matrix from the (n - 2)-th frame of lidar point cloud p n-2 to p n-1 , R, t n-1 , θ, x n-1 , y n-1 are respectively the rotation transformation matrix, translation transformation matrix, rotation angle, translation distance in the x direction, and translation distance in the y direction from p n-1 to p n .

[0068] In another embodiment, obtaining p1 and p n The pose transformation matrix T1 n It can also be implemented using existing algorithms such as ICP, PL-ICP, and IMLS-ICP.

[0069] In one embodiment, the current n-th frame LiDAR pose p can be obtained according to formula (4): n , the nth frame LiDAR pose can also be obtained by multiplying the first frame LiDAR pose by the first frame to nth frame pose transformation matrix.

[0070] p n =T1 n p1 (4)

[0071] Step 3, synchronize the coordinate system: first calibrate the position of the magnetic sensor M relative to the lidar L For example, the approximate installation positions of the laser radar L and the magnetic sensor M on the industrial intelligent robot can be measured manually, and then the coordinates of the magnetic sensor M can be converted to the laser radar coordinate system through the existing hand-eye calibration algorithm. The external parameter data of the magnetic sensor M relative to the laser radar L is used to represent the position of the magnetic sensor M relative to the laser radar L.

[0072] Then according to step 1 T1 obtained in step 2 n and p n , using formula (5), the prior coordinates of the first magnetic nail and the mth magnetic nail are synchronized to the global coordinates Mag1 and Mag2 in the laser radar coordinate system respectively. m ;

[0073]

[0074] Step 4: Construct a local map: Based on the T obtained in step 1 m 、p n With p n-1 The time difference Δt=t n -t n-1 , use formula (6) to calculate the speed V of the magnetic sensor M passing the mth magnetic nail m , further optimize the pose estimation of the lidar L, and construct a map containing landmark points Mag every preset number of frames (for example: 3000) m Local map of:

[0075] V m =(T m -T m-1 ) / Δt (6)

[0076] In formula (6), T m-1is the pose change matrix of the origin of the magnetic sensor M in the (m - 1)-th frame relative to the first magnetic nail, T m is the pose transformation matrix of the origin of the magnetic sensor M in the m-th frame relative to the first magnetic nail.

[0077] In one embodiment, the velocity V of the magnetic sensor M passing through the m-th magnetic nail m can also be calculated by Equation (7):

[0078]

[0079] In Equation (7), (x m , y m ), (x m-1 , y m-1 ) are respectively the positions of the magnetic sensor M in the lidar coordinate system at the time of the (m - 1)-th frame of laser point cloud. Of course, the velocity V of the magnetic sensor M passing through the m-th magnetic nail m can also be obtained by other existing methods.

[0080] In one embodiment, the method for further optimizing the pose estimation of the lidar specifically includes:

[0081] According to T obtained in step 1 m , when p obtained in step 2 n and Mag obtained in step 3 m meet the condition of Equation (9), update p n to Mag m , and use Mag m as a landmark;

[0082]

[0083] In the formula, D m is the absolute distance between p n and p n-1 , C is a constant, which can be selected as 0.5, or can be adjusted to other values according to the actual situation.

[0084] Step 5, record each local map, perform loop detection, and generate a global map.

[0085] The method provided in the above embodiment uses lidar and magnetic scale encoder devices. The processes of the positioning and mapping functions are as Figure 1 shown in the positioning and mapping flow chart. By using the information of the magnetic encoder and the magnetic nails to assist the single-line lidar positioning, it provides landmarks and high-precision velocity values for the single-line lidar positioning, thereby solving the problems of single-line lidar positioning loss and odometer degradation in the case of few large structural features in space.

[0086] In the above embodiments, before step 2, the map building step further includes a step of removing distortion from the point cloud. The step of removing distortion from the point cloud can use a uniform motion model to process the point cloud data collected by the lidar, so as to avoid the defect that the time delay during the operation of the lidar drive affects the real-time performance of the point cloud.

[0087] In one embodiment, as Figure 2 shown, the positioning step specifically includes:

[0088] Step a, loading the global map: loading the global map generated in step 5;

[0089] Step b, removing distortion from the point cloud: using a uniform motion model to process the lidar data to remove distortion when there is no participation of magnetic nail positioning, so as to avoid the defect that the time delay during the operation of the lidar drive affects the real-time performance of the point cloud.

[0090] Step c, matching the point cloud with the map: using Mag m as the initial value to match the lidar point cloud information with the grid map when the magnetic sensor passes by the magnetic nail;

[0091] Step d, optimizing the pose information: traversing the magnetic nail coordinates in the manner of formula (9), and using the magnetic nail coordinates and the matching result of the point cloud with the map to optimize the pose by weight ratio when passing by the magnetic nail;

[0092] Step e, publishing the pose information: customizing ROS messages and publishing the current pose and real-time speed information in the global coordinate system.

[0093] The embodiment of the present invention further provides a positioning and mapping device based on a lidar and a magnetic sensor, which includes a mapping unit and a positioning unit;

[0094] Among them, the mapping unit specifically includes a magnetic nail related information acquisition sub-unit, a coordinate system synchronization sub-unit, a local map construction sub-unit and a global map generation unit:

[0095] The magnetic nail related information acquisition sub-unit is used to arrange magnetic nails in the working site in the selected unstructured scene, obtain the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail is and the pose of the m-th magnetic nail relative to the origin of the magnetic sensor Furthermore, according to formula (1), obtain the pose transformation matrix T of the origin of the m-th frame magnetic sensor M relative to the first magnetic nail m , where the prior coordinates of the first magnetic nail are (0, 0), and the prior coordinates of the m-th magnetic nail are determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, m≠1.

[0096] The coordinate system synchronization sub-unit is used to calibrate the pose of the magnetic sensor M relative to the lidar L Obtained according to Step 1 T1 obtained in Step 2 n and p n , synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m .

[0097] The local map construction subunit is used to calculate the speed V of the magnetic sensor M passing through the m-th magnetic nail according to T obtained in Step 1 m , p n and the time difference Δt = t n-1 -t n between p n-1 , further optimize the pose estimation of the lidar L, and construct a local map containing the landmark Mag every preset number of frames m . m

[0098] The global map generation unit is used to record each local map, perform loop detection, and generate a global map

[0099] In one embodiment, the pose solution operator unit specifically includes a pose transformation matrix module and a lidar point cloud estimation module, where:

[0100] The pose transformation matrix module is used to obtain the relative translation distance and relative rotation angle in the x and y directions in the global coordinate system according to the poses of two adjacent frames of lidar point clouds collected by the lidar L, and obtain p according to the following formulas (2) and (3). First, obtain the pose transformation matrix of the previous frame of lidar point cloud p n and p n according to formula (2), and then obtain the pose transformation matrix T1 of p1 and p n-1 according to formula (3). Then obtain the pose transformation matrix T1 of p1 and p n according to formula (3). n .

[0101] The lidar point cloud estimation module is used to obtain the current n-th frame of lidar point cloud p n according to formula (4).

[0102] In one embodiment, the local map construction subunit is specifically used to calculate the speed V of the magnetic sensor M passing through the m-th magnetic nail by using formula (6), further optimize the pose estimation of the lidar L, and construct a local map containing the landmark Mag every preset number of frames m . m

[0103] In one embodiment, the method for further optimizing the pose estimation of the lidar by the local map construction subunit specifically includes:

[0104] According to the T obtained by the magnetic nail-related information acquisition subunit​​m , when p obtained by the pose resolver unit n and Mag obtained by the coordinate system synchronizer unit m meet the condition of Equation (9), update p n to Mag m , and use Mag m as a landmark.

[0105] The present invention solves the problems of positioning loss and odometer degradation caused by the limitation of the laser radar positioning scanning range. The method of fusing magnetic nail positioning can provide both landmark information and speed information. Different from the current methods on the market that fuse wheel speedometers and vision, as well as the method of single magnetic nail positioning, it has the advantages of high positioning accuracy, strong robustness, sparse magnetic nail arrangement, and flexible robot driving path.

[0106] Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Those of ordinary skill in the art should understand that the technical solutions recorded in the foregoing embodiments can be modified, or some of the technical features can be equivalently replaced; these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning and mapping method based on lidar and magnetic sensors, characterized in that, It includes a mapping step and a positioning step; Among them, the mapping step specifically includes: Step 1, arrange magnetic nails at the working site in the selected unstructured scenario, and obtain the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, and the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail is and the pose of the m-th magnetic nail relative to the origin of the magnetic sensor Furthermore, obtain the pose transformation matrix T of the origin of the m-th frame magnetic sensor M relative to the first magnetic nail according to Equation (1) m , where the prior coordinates of the first magnetic nail are (0, 0), the prior coordinates of the m-th magnetic nail are determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, and m≠1; Step 2: Obtain the current n-th frame of lidar point cloud p according to the change in the poses of two adjacent frames of lidar point clouds collected by lidar L n , p1 and p n pose transformation matrix of where n = m + 1; Step 3: Calibrate the pose of the magnetic sensor M relative to the lidar L Obtained according to Step 1 Obtained in Step 2 and p n , synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m ; Step 4, according to T obtained in Step 1 m , p n and the time difference Δt = t n-1 - t n of p n-1 , calculate the speed V of the magnetic sensor M passing through the m-th magnetic nail m , further optimize the pose estimation of the lidar L, and construct a local map containing the landmark Mag every preset number of frames m : Step 5, record each local map, perform loop detection, and generate a global map.

2. The positioning and mapping method based on lidar and magnetic sensor according to claim 1, wherein In step 2, the pose transformation matrix T1 of p1 and p n is obtained as follows: n The specific method for obtaining According to the relative translation distance and relative rotation angle in the x and y directions of the poses of two adjacent frames of lidar point clouds collected by lidar L in the global coordinate system, they are obtained using the following formulas (2) and (3). First, obtain p according to formula (2) n and p n of the previous frame of lidar point cloud p n-1 pose transformation matrix Then, obtain the pose transformation matrix T1 of p1 and p n according to formula (3) n : In Equations (2) and (3), is p n and p n is the pose transformation matrix of the previous frame of laser point cloud p n-1 ; is the pose transformation matrix from the (n - 2)-th frame of laser point cloud p n-2 to p n-1 , and R, t n-1 , θ, x n-1 , y n-1 are respectively the rotation transformation matrix, translation transformation matrix, rotation angle, translation distance in the x direction, and translation distance in the y direction from p n-1 to p n .

3. The positioning and mapping method based on lidar and magnetic sensors according to claim 2, wherein Obtain the current nth-frame laser point cloud p according to Equation (4) n :[[-END]] p n = T1 n p1(4).

4. The positioning and mapping method based on lidar and magnetic sensors according to claim 1, wherein In step 3, according to what is obtained in step 1 T1 obtained in step 2 n and p n , using Equation (5), synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m ; 5. The positioning and mapping method based on lidar and magnetic sensors according to any one of claims 1-4, characterized in that In step 4, the velocity V of the magnetic sensor M passing through the m-th magnetic nail is calculated using Equation (6). m , the pose estimation of the lidar L is further optimized, and a local map containing the road marking point Mag is constructed every preset number of frames m : V m = (T m - T m-1 ) / Δt (6) In formula (6), T m-1 is the pose change matrix of the origin of the magnetic sensor M in the (m - 1)-th frame relative to the first magnetic nail.

6. The positioning and mapping method based on lidar and magnetic sensors according to claim 5, characterized in that In Step 4, the method for further optimizing the pose estimation of the lidar specifically includes: T obtained according to Step 1 m , p obtained in Step 2 n and Mag obtained in Step 3 m Under the condition that they conform to Equation (9), update p n to Mag m , and use Mag m as a waypoint; where D m is the absolute distance between p n and p n-1 , and C is a constant.

7. A positioning and mapping device based on lidar and magnetic sensors, characterized in that, It includes a mapping unit and a positioning unit; Among them, the mapping unit specifically includes: The magnetic nail related information acquisition subunit is used to arrange magnetic nails in the working site under the selected unstructured scenario, and obtain the prior coordinates of the first magnetic nail and the current m-th magnetic nail in the magnetic nail coordinate system, and the pose transformation matrix of the m-th magnetic nail relative to the first magnetic nail is and the pose T of the m-th magnetic nail relative to the origin of the magnetic sensor m M , and then obtain the pose transformation matrix T of the origin of the m-th frame magnetic sensor M relative to the first magnetic nail according to formula (1) m , where the prior coordinates of the first magnetic nail are (0, 0), and the prior coordinates of the m-th magnetic nail are determined by the relative position relationship between the m-th magnetic nail and the first magnetic nail, m≠1; A pose resolver unit, which is used to obtain the current nth frame of lidar point cloud p according to the change in the pose of two adjacent frames of lidar point clouds collected by lidar L n 、p1 and p n pose transformation matrix T1 n , where n = m + 1; A coordinate system synchronization subunit, which is used to calibrate the pose T of the magnetic sensor M relative to the lidar L L M , according to the obtained in step 1 m 1T mag , T1 obtained in step 2 n and p n , synchronize the prior coordinates of the first magnetic nail and the m-th magnetic nail to the global coordinates Mag1 and Mag in the lidar coordinate system respectively m ; Local map construction subunit, according to T obtained in step 1 m , p n and the time difference Δt = t n-1 - t n of p, calculate the speed V of the magnetic sensor M passing through the m-th magnetic nail n-1 , further optimize the pose estimation of the lidar L, and construct a local map including the landmark Mag m at every preset number of frames: m ​ A global map generation unit, which is used to record each local map, perform loop detection, and generate a global map.

8. The positioning and mapping device based on lidar and magnetic sensor according to claim 7, characterized in that, The pose resolver unit specifically includes: A pose transformation matrix module, which is used to obtain the relative translation distance and relative rotation angle in the x and y directions in the global coordinate system according to the poses of two adjacent frames of lidar point clouds collected by lidar L, and is obtained by the following formulas (2) and (3). First, obtain p according to formula (2) n and p n the previous frame of lidar point cloud p n-1 pose transformation matrix of Then obtain the pose transformation matrix T1 of p1 and p according to formula (3) n n :​ In Formula (2) and Formula (3), is p n and p n is the pose transformation matrix of the previous frame of laser point cloud p n-1 ; is the pose transformation matrix from the (n - 2)-th frame of laser point cloud p n-2 to p n-1 , and R, t n-1 , θ, x n-1 , y n-1 are respectively the rotation transformation matrix, translation transformation matrix, rotation angle, translation distance in the x direction, and translation distance in the y direction from p n-1 to p n ; Laser point cloud estimation module, which is used to obtain the current nth frame of laser point cloud p according to Equation (4) n :[[]]END]] p n = T1 n p1(4).

9. The positioning and mapping device based on lidar and magnetic sensors according to claim 7 or 8, characterized in that The local map construction subunit is specifically used to calculate the velocity V of the magnetic sensor M passing through the m-th magnetic nail by using Equation (6), further optimize the pose estimation of the lidar L, and construct a local map including the waypoint Mag every preset number of frames: m , and further optimize the pose estimation of the lidar L, and construct a local map including the waypoint Mag every preset number of frames m : V m = (T m - T m-1 ) / Δt (6) In formula (6), T m-1 is the pose change matrix of the origin of the magnetic sensor M in the (m - 1)-th frame relative to the first magnetic nail.

10. The positioning and mapping device based on lidar and magnetic sensor according to claim 9, characterized in that, The method for further optimizing the pose estimation of the lidar by the local map construction sub-unit specifically includes: T obtained by the sub-unit for acquiring magnetic nail related information m , p obtained by the pose solving sub-unit n and Mag obtained by the coordinate system synchronization sub-unit m When meeting the condition of formula (9), update p n to Mag m , and use Mag m as a landmark; Where D m is the absolute distance between p n and p n-1 , and C is a constant.

Images