A mobile robot repositioning method and device based on visual features and 3D laser

Abstract

The application discloses a kind of mobile robot repositioning and the method of device based on visual feature and 3D laser, this method first needs to carry out environment mapping and Apriltag label world coordinate automatic calibration;When mobile robot loses positioning, left and right camera detection label function is started, stop moving after detecting label;The relative pose between mobile robot and label is acquired, and filtering is carried out in a period of time, to ensure the stability of pose data;According to the label world coordinate obtained by calibration, the world coordinate where robot is located is recovered, and preliminary repositioning is realized;Directional range search is carried out with the world coordinate as center, and the 3D point cloud frame data obtained by scanning is continuously matched with existing subgraph, to realize accurate repositioning.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of mobile robot repositioning, and particularly relates to a mobile robot repositioning method and device based on visual features and 3D laser. BACKGROUND

[0002] With the rapid development of modern logistics and warehousing industry, the reform of using mobile robots to replace manual handling is gradually carried out, and more and more intelligent warehousing robots are stepping into the warehouse of the logistics industry to undertake the task of autonomous handling, further promoting the unmanned and intelligent of the logistics warehouse. The core problem of the intelligent warehousing robot executing the handling task in the actual scene is how to position according to the sensor carried by itself and the environment information, and to distribute, plan and navigate the task according to the positioning information. The positioning information is the basis for the movement of the warehousing robot. In recent years, the development of SLAM technology makes it possible for the warehousing robot to position autonomously. However, the warehousing robot may encounter the problem of losing positioning in the actual operation process, which is specifically manifested as: the warehousing robot is forced to restart due to internal or external abnormal reasons, and the dynamic environment causes the estimated pose information of the warehousing robot to be wrong. At the same time, using a 3D SLAM scheme based on multi-line laser instead of a 2D SLAM scheme based on single-line laser can make better use of environmental information and improve the repositioning ability of the warehousing robot. Combined with visual feature information, the repositioning accuracy can be further improved on the basis of only environmental information. Therefore, researching a 3D laser SLAM repositioning method based on visual features can better solve the problem of robot repositioning. SUMMARY

[0003] The purpose of the present application is to improve the repositioning ability of the warehousing robot. On the basis of introducing multi-line laser information to overcome the limitation of 2D plane, further combined with visual feature information, the repositioning accuracy of the warehousing robot is improved. Therefore, the present application provides a mobile robot repositioning method and device based on visual features and 3D laser. The method relates to a monocular camera, a multi-line laser radar and an IMU sensor. The monocular camera is used for detecting Apriltag label information, the multi-line laser radar is used for collecting point cloud data of the environment, and the IMU is used for sensing the gravity direction and providing speed and angular velocity information. By calibrating the existing environment, when repositioning is needed after losing positioning, the Apriltag label information is detected for preliminary repositioning, and appropriate transformation of the pose is performed on this basis. The current scanning frame point cloud data, IMU information and existing subgraph information are matched to select the optimal transformation and realize accurate positioning.

[0004] To achieve the above purpose, the technical scheme of the present application is as follows: in a first aspect, the present application provides a mobile robot repositioning method based on visual features and 3D laser, comprising the following steps:

[0005] S1: 3D mapping of the actual scene is performed using a multi-line laser radar and an IMU sensor. In the process of map construction, the pose of the mobile robot relative to the Apriltag label is obtained, the world coordinate information of the Apriltag label is obtained according to the world coordinate conversion of the mobile robot, and the label number is recorded, so as to complete the calibration of the world coordinate of the Apriltag label;

[0006] S2: When the mobile robot loses its own positioning, the left and right cameras on the mobile robot constantly acquire the left and right images of the mobile robot. When an Apriltag label is detected in the field of view of any camera, the movement of the mobile robot is stopped.

[0007] S3: A plurality of pose data of the mobile robot relative to the Apriltag label is obtained, denoted as a set S:

[0008]

[0009] wherein x i , y i , and z i represent displacement transformations in X, Y, and Z axis directions relative to the Apriltag label, φ i represent angle transformations in X, Y, and Z axis directions relative to the Apriltag label, and n represents the total number of acquired poses.

[0010] S4: The world coordinates of the label calibrated in S1 are looked up according to the acquired Apriltag label number, denoted as Based on the pose data relative to the Apriltag label obtained in S3, the world coordinates of the mobile robot are calculated, and the preliminary repositioning of the mobile robot is realized. The preliminary repositioning result of the mobile robot is denoted as W2.

[0011] S5: The coordinates of W2 are combined with the multi-line laser point cloud information for further calculation. Specifically, first, the current frame point cloud data of the 3D laser on the mobile robot is subjected to voxel filtering and noise removal. The space where the mobile robot is located is divided into cubic grids of a fixed side length to form a grid space. The center value of the cubic grid is set to 1 and 0 according to whether the points in the point cloud data obtained by 3D laser scanning are in the cubic grid. The values of the remaining points in the cubic grid are calculated through the center values of the adjacent cubic grids. The value size represents the probability of obstacles. Then, taking the coordinates W2 as the center and taking the parameter r sFor the radius and combined with the angle information of W2 relative to W1 in S4, the directional range search is performed. The current frame point cloud data is transformed in 3D layer according to equal interval displacement and angle, and the probability value is obtained according to the coordinates of the transformed points in the grid space, the cumulative sum of the probability value is calculated, and when the cumulative sum is the maximum value, the interval transformed pose data corresponding to the maximum value is taken as the accurate repositioning result, that is

[0012] Further, step S1 specifically comprises the following steps:

[0013] S11: The point at which the mobile robot starts mapping is taken as the origin of the world coordinate system. In the moving process, the point cloud data obtained by 3D laser scanning on the mobile robot is matched with the point cloud data obtained in the previous scanning process. A subgraph set strategy is adopted to establish a subgraph with N frames of point cloud data, and a node is constructed with every N frames of mobile robot pose. In the process of continuous movement of the mobile robot, the transformation process constraint between nodes and the constraint between the current frame and the adjacent node are continuously calculated, the constraint is taken as the edge between nodes, and nonlinear optimization is performed to convert it into a graph optimization problem, and the optimization function is constructed as follows:

[0014]

[0015] In the above formula, respectively represent the subgraph set with constraint relationship of the current scanning frame and the pose set of the obtained scanning frame, n1 and n2 respectively represent the number of subgraphs with constraint relationship with the current scanning frame and the number of scanning frames before the current scanning frame, and Δ ij , Ω ij respectively represent the relative pose and covariance matrix of scanning frame j relative to subgraph i, and E(·) represents the nonlinear error function about Ω ij , Δ ij , the Levenberg-Marquardt algorithm is used to solve the optimal subgraph pose p m and the optimal scanning frame pose p s , and the initial pose of the mobile robot is converted into the world coordinate of the mobile robot at the current time;

[0016] S12: In the process of building the map, the left and right cameras continuously acquire images for detection. When an Apriltag label is detected, 0 and 1 are used to distinguish the data recorded by the left and right cameras. The label number detected at this moment and the world coordinates of the Apriltag label obtained by converting the world coordinates of the mobile robot are recorded and saved for subsequent use.

[0017] Further, step S4 specifically comprises the following steps:

[0018] S41: Based on whether the left and right cameras detect the tag during the actual movement of the mobile robot, the situation can be divided into two categories: one is single-sided, and the other is double-sided. Single-sided means that only the left or right camera detects the tag, then proceed to step S42. Double-sided means that both the left and right cameras detect the tag, then proceed to step S43.

[0019] S42: In the case of a single-sided camera, when multiple tags are detected within the camera's field of view, the detected tag number is d. i i = 1, ..., m, where m represents the number of detected tags; the world coordinates of the corresponding calibrated tag are found based on the tag number. Take the relative displacement transformation information from the pose data of the mobile robot relative to the tag obtained at this time. Angle information and The initial world coordinates of the mobile robot are obtained as follows: angle

[0020] S43: In the case of bilateral detection, record the tag number obtained from the left side as l. i i = 1, ..., m l , where m l The left side represents the number of detected labels, and the right side represents r. j j = 1, ..., m r , where m r This indicates the number of detected labels on the right. The world coordinates of the corresponding label are found based on the label number. Take the relative displacement transformation information from the pose data of the mobile robot relative to the tag obtained at this time. Angle information and The initial world coordinates of the mobile robot are obtained as follows: angle

[0021] Furthermore, step S5 specifically includes the following steps:

[0022] S51: World coordinates of the mobile robot obtained from S4 Let r be the center of the sphere. s Radius, Angle and Create a spherical cone with parameter r s It can be customized according to the actual environment; within the range of the spherical cone, it is set according to several equal intervals. The point cloud data obtained by the current frame scanning of 3D laser is transformed in 3D space. The point cloud data before the transformation is denoted as C, and the point cloud data after the transformation is denoted as C′.

[0023] S52: using the grid idea, the space where the current mobile robot is located is divided into a fixed edge length cube, and the value at the center of the cube represents the probability that the cube is an obstacle; the higher the probability value, the darker the cube, indicating that the location is more likely to be an obstacle, and the lower the probability, the more likely the location is empty, and the cube in the unknown area is set to gray, i.e. the probability value is 0.5.

[0024] S53: according to the set different interval ΔT, the current scanning point cloud data C is transformed, and the probability value is obtained according to the coordinates of the transformed points in the grid space, the probability value of the current point cloud data point is accumulated, and the maximum ΔT of the probability value is defined as the optimal transformation ΔT', and the probability sum formula is as follows:

[0025]

[0026] According to the maximum probability sum formula, a least square model is established for nonlinear optimization, and a loss function is introduced, so as to obtain the optimal transformation ΔT'.

[0027]

[0028] Wherein, K represents the number of point clouds obtained by scanning at t time, and p k represents the loss function for excluding the influence of abnormal values on the optimization result, s represents the probability value of the cube, Q represents the calculation probability function, and h k represents the coordinates of the kth point in the point cloud obtained by scanning at t time in the mobile robot coordinate system, and ΔT represents the transformation matrix for transforming the coordinates of the kth point in the mobile robot coordinate system into the coordinates in the world coordinate system.

[0029] S54: based on the optimal transformation is transformed into as the final accurate positioning coordinates.

[0030] In a second aspect, the application further provides a mobile robot repositioning device based on visual features and 3D laser, comprising a memory and one or more processors, the memory stores executable code, and the processor executes the executable code to realize the mobile robot repositioning method based on visual features and 3D laser.

[0031] In a third aspect, the application further provides a computer readable storage medium having a program stored thereon, wherein the program is executed by a processor to realize the mobile robot repositioning method based on visual features and 3D laser.

[0032] ​The application can effectively solve the positioning loss problem of the mobile robot caused by abnormality, rough positioning is performed through sensing visual features, the repositioning precision is further improved on the basis of combining 3D laser information, accurate positioning is realized, the mobile robot can recover the sensing ability and positioning ability of the surrounding environment in a short time, so that the mobile robot can continue to complete the production task. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 A flowchart for the method of the application is shown.

[0034] Figure 2 An Apriltag label diagram is shown.

[0035] Figure 3 A schematic diagram of the relative positions of the robot and the sensor is shown.

[0036] Figure 4 A schematic diagram of the point cloud transformation range is shown.

[0037] Figure 5 A schematic diagram of the grid selection is shown.

[0038] Figure 6 A structural diagram of the mobile robot repositioning device based on visual features and 3D laser of the application is shown. DETAILED DESCRIPTION

[0039] The execution steps of the method will be described in further detail below in conjunction with the drawings, but the protection scope of the application is not limited to the following description.

[0040] As shown in Figure 1 , the application proposes a mobile robot repositioning method based on visual features and 3D laser, which includes the following steps:

[0041] The multi-line laser radar used in the embodiment of the application is a QUANERGY M8 series 16-line laser radar, the left and right cameras use USB cameras after calibration parameters, the IMU is a wheel interest technology IMU, and the schematic diagram of the relative positions of the sensors is shown in Figure 3 .

[0042] S1: 3D mapping of the actual scene is performed by using the multi-line laser radar and the IMU, the map construction of the 3D SLAM system is performed by using real-time correlation scanning matching, nonlinear back-end optimization, branch and bound acceleration loop detection, the pose of the mobile robot relative to the Apriltag label is acquired, the Apriltag label world coordinate information is obtained according to the mobile robot world coordinate conversion, the label number is recorded, the world coordinate calibration of the Apriltag label is completed, and the Apriltag label diagram is shown in Figure 2 .

[0043] S2: When the mobile robot loses its own positioning, it continuously acquires images of the left and right sides of the mobile robot through the left and right cameras located on the mobile robot. If no tag is detected for a long time, it can move back and forth slightly. Once a tag is detected within the field of view of either camera, it stops moving.

[0044] S3: Acquire the relative pose data obtained by the camera within 6 seconds, denoted as set S:

[0045]

[0046] Where x i y i , z i These represent displacement transformations along the X, Y, and Z axes, respectively. φ i represents the angle transformations along the X and Z axes, respectively, and n represents the total number of poses acquired within 6 seconds. Let p i After removing outliers from the set, mean filtering is applied to obtain the mean. This reduces the impact of local outliers on the overall judgment result.

[0047] S4: The left and right cameras continuously detect labels in the real-time acquired images. After detecting a label, they calculate the relative pose data, which includes relative displacement information and relative angle information. The relative displacement information refers to the displacement of the camera's current position relative to the label in the X, Y, and Z axis directions, while the relative angle information refers to the deflection angle of the camera's current position relative to the label in the X, Y, and Z axis directions.

[0048] Find the world coordinates of the tag obtained in step S1 according to the acquired tag number, and denot them as follows. Based on different actual situations, the world coordinates of the mobile robot are calculated to achieve the initial relocalization of the mobile robot. This point is denoted as...

[0049] S5: The pose calculated based on the label is subject to certain deviations due to lighting and the actual environment. The coordinates of W2 need to be further calculated by combining multi-line laser point cloud information. First, voxel filtering and noise removal are performed on the current frame point cloud data. The purpose of voxel filtering is to use local point clouds with obvious features to replace the global point cloud, reducing the overall amount of point cloud data that needs to be processed. The purpose of noise removal is to remove abnormal point cloud data and reduce the adverse effects on the results. The space where the current mobile robot is located is divided into cubic grids with fixed side lengths to form a grid space. Based on whether the points in the point cloud data obtained from 3D laser scanning are inside the cubic grid, different values ​​are assigned to the center of the cubic grid. The value of the center of the cubic grid represents the probability that the cubic grid is an obstacle. Then, with point W2 as the center, and parameter rs For the radius and combined with the angle information of W2 relative to W1 in step S4, the directional range search is performed. The current frame point cloud data is transformed in 3D layer according to equal interval displacement and angle, and the probability value is obtained according to the coordinates of the transformed points in the grid space, the cumulative sum of the probability value is calculated, and the interval transformed pose data corresponding to the maximum cumulative sum is taken as the accurate repositioning result, that is

[0050] Further, step S1 specifically comprises the following steps:

[0051] S11: as Figure 3 As shown in the schematic diagram, the Apriltag label height is within 10 cm up and down of the mobile robot camera installation height, the point at which the mobile robot starts mapping is taken as the origin of the world coordinate system, in the moving process, the point cloud data obtained by the 3D laser scanning on the mobile robot is matched with the point cloud data obtained in the previous scanning process, a subgraph set strategy is adopted, N frames of point cloud data are used to establish a subgraph, and a node is constructed every N frames of pose, in the process of continuous movement of the mobile robot, the transformation process constraint between nodes and the constraint between the current frame and the adjacent node are calculated, the constraint is taken as the edge between nodes, nonlinear optimization is performed in the back end of the 3D SLAM system, which is converted into a graph optimization problem, and the optimization function is constructed as follows:

[0052]

[0053] In the above formula, respectively represent the subgraph set with constraint relationship of the current scanning frame and the pose set of the obtained scanning frame, n1 and n2 respectively represent the number of subgraphs with constraint relationship with the current scanning frame and the number of previous scanning frames, Δ ij , Ω ij respectively represent the relative pose and covariance matrix of scanning frame j relative to subgraph i, E(·) represents Ω ij , Δ ij nonlinear error function, the Levenberg-Marquardt algorithm is used to solve the optimal subgraph pose p m and the optimal scanning frame pose p s , which is converted into the world coordinate at the current time combined with the initial pose of the mobile robot.

[0054] S12: in the process of building the map, the left and right cameras continuously acquire images for detection, when the Apriltag label is detected and directly opposite the label, the data recorded by the left and right cameras are distinguished by 0 and 1, the label number detected at this moment is recorded, and the world coordinates of the Apriltag label are obtained according to the world coordinate conversion of the mobile robot and saved for subsequent use.

[0055] Further, step S3 specifically includes the following steps:

[0056] S31: The abnormal value is set to 5 times or more of the current mean value, and the data is discarded if an abnormal value is detected. The mean filtering process is adopted for the obtained multiple normal data, and the formula is as follows:

[0057]

[0058] wherein, is the final obtained mean pose data, p i is the non-abnormal pose data.

[0059] Further, step S4 specifically includes the following steps:

[0060] S41: During the actual movement of the robot, the left and right cameras may not always be able to detect the label. According to whether the label is detected, the situation can be divided into two categories: one is single-sided, and the other is double-sided. Single-sided means that only the left camera or the right camera detects the label, i.e. Figure 3 as shown in the schematic diagram, only the label on the left or right wall enters the field of view of the single-sided camera, and double-sided means that both the left and right cameras detect the label, i.e. both the left and right cameras can detect the corresponding label on the respective wall.

[0061] S42: In the single-sided case, when multiple labels are detected within the camera range, the detected label number is denoted as d i , i = 1, …, m, where m represents the number of detected labels. Since the field of view is limited, m is generally not more than 5. According to the label number, the corresponding world coordinates obtained from calibration are searched as The relative displacement transformation information in the relative pose data detected at this time is taken The angle information and The preliminary world coordinates of the mobile robot are obtained as Angle

[0062] S43: In the double-sided case, the left detected label number is denoted as l i , i = 1, …, m l , where m l represents the number of left detected labels, and the right is r j , j = 1, …, m r , where m r represents the number of right detected labels. According to the label number, the corresponding world coordinates in the calibration information are searched as The relative displacement transformation information in the relative pose data detected at this time is taken Angle information and The initial world coordinates of the mobile robot are obtained as follows: angle

[0063] Further, step S5 includes the following steps:

[0064] S51: World coordinates of the mobile robot obtained in step S4 Let r be the center of the sphere. s Radius, Angle and Construct a spherical cone, such as Figure 4 As shown, the transformation range is not the entire sphere. Performing the transformation within the conical sphere ensures correct directional matching and significantly reduces the amount of transformation. The parameter r... s It can be customized according to the actual environment. Within the spherical cone area, several equally spaced intervals are set. Perform a 3D spatial transformation on the point cloud data obtained from the current frame scan. Let the point cloud data before the transformation be C, and the point cloud data after the transformation be C′.

[0065] S52: Using the concept of gridding, the space where the mobile robot is located is divided into cubic grids with fixed side lengths. The value at the center of the cubic grid represents the probability that the grid is an obstacle. The generalization formula for calculating the probability value q is as follows:

[0066] q=A*(2t 3 -3t 2 +1)+B*(-2t 3 +3t 2 )

[0067] Where A and B are constant coefficients, when t=0, A=1, B=0; when t=1, A=0, B=1. The process of determining the constant coefficients A and B is as follows:

[0068] After performing a ΔT transformation on point k, the map coordinates are obtained as (x m y m , z m ),like Figure 5 As shown, query the 3D occupied raster map (x m y m , z m The corresponding raster number (x) index y index , z index ), calculate its raster center (x) based on the raster number. c y c , z c ), with x m >x c, y m > y c , z m > z c Take an example for analysis, record (x c , y c , z c ) as (x1, y1, z1), add resolution r to (x c , y c , z c ) to get (xc+r, y c +r, z c +r), which is recorded as (x2, y2, z2), resolution r can be set autonomously, which is set to 5cm here, if x m x c , y m y c , z m z c , then (x2, y2, z2) = (x c -r, y c -r, z c -r).

[0069] Calculate the normalized coordinates (x n , y n , z n ) according to (x1, y1, z1) and (x2, y2, z2).

[0070] (x n , y n , z n ) = ((x-x1) / (x2-x1), (y-y1) / (y2-y1), (z-z1) / (z2-z1))

[0071] Get the label number (x index1 , y index1 , z index1 ) of the grid where (x1, y1, z1) is located, get the grid probability q 111 according to the label number, and add 1 to the label respectively, for example: (x index1 +1, y index1 , z index1 ), get the label number of the remaining 7 points, get the probability according to the transformed label number to get q 112 , q 121 , q 122 , q 211 , q 212 , q 221 , q 222 , the value is 0 or 1, according to the obtained 8 probability values, get the point (x m , y m , zm The specific form of the probability q of the formula:

[0072]

[0073]

[0074]

[0075]

[0076]

[0077]

[0078]

[0079] By comparing the general formula of the probability value q with the specific form of the formula, it can be known that the specific form of the constant coefficients A and B, the higher the probability value, the darker the cubic grid, which represents that the position is more likely to be an obstacle, and the lower the probability, which represents that the position is more likely to be empty. The cubic grid at the unknown area is set to gray, i.e. the probability value is 0.5.

[0080] S53: After the existing scanning space is rasterized and the probability calculation method is set, the current scanning point cloud data C is transformed according to the set different intervals ΔT, and the probability value is obtained according to the coordinates of the transformed points in the grid space. The probability value is accumulated, and the maximum ΔT of the accumulated sum of the probability value is defined as the optimal transformation ΔT', and the probability sum formula is as follows:

[0081]

[0082] According to the maximum probability sum formula, a least square model is established for nonlinear optimization, and a loss function is introduced, so as to obtain the optimal transformation ΔT'.

[0083]

[0084] Wherein, K represents the number of point clouds obtained by scanning at t time, ρ k represents the loss function is used to exclude the influence of abnormal values on the optimization result, s represents the probability value of the cubic grid, Q represents the calculation probability function, h k represents the coordinates of the point k in the mobile robot coordinate system in the point cloud obtained by scanning at t time, and ΔT represents the transformation matrix for transforming the coordinates of the point k in the mobile robot coordinate system into the coordinates in the world coordinate system.

[0085] S54: The transformation with the highest probability sum obtained by calculation is recorded as the optimal transformation, and transformed into As the coordinate of the final accurate positioning, after obtaining the coordinate, it means that the mobile robot can be positioned at the lost pose with small error, ensuring that the front end of the 3D SLAM system can be corrected within the matching error range through the back end nonlinear optimization and loop detection of the 3D SLAM system, and without the case of random matching and lost positioning.

[0086] Corresponding to the above-mentioned embodiments of the mobile robot repositioning method based on visual features and 3D laser, the application also provides embodiments of the mobile robot repositioning device based on visual features and 3D laser.

[0087] Referring to Figure 6 , the embodiment of the application provides a mobile robot repositioning device based on visual features and 3D laser, comprising a memory and one or more processors, the memory stores executable code, and the processor executes the executable code to implement the mobile robot repositioning method based on visual features and 3D laser in the above-mentioned embodiments.

[0088] The embodiment of the mobile robot repositioning device based on visual features and 3D laser of the application can be applied to any device with data processing capability, which can be a device or apparatus such as a computer. The device embodiment can be realized by software, or by hardware or a combination of software and hardware. Taking software realization as an example, as a logical device, it is formed by reading the corresponding computer program instructions in the non-volatile memory into the memory and running by the processor of the device with data processing capability. From the hardware level, as shown in Figure 6 , it is a hardware structure diagram of the device with data processing capability of the mobile robot repositioning device based on visual features and 3D laser of the application, in addition to the processor, memory, network interface and non-volatile memory shown in Figure 6 , the device with data processing capability in the embodiment usually includes other hardware according to the actual function of the device with data processing capability, and details are not described here.

[0089] The implementation process of the functions and roles of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and details are not described here.

[0090] For the device embodiment, since it basically corresponds to the method embodiment, the relevant part can be seen from the part of the method embodiment. The device embodiment described above is only illustrative, wherein the units described as separate components can or can not be physically separated, and the components displayed as units can or can not be physical units, i.e., can be located in one place or distributed to multiple network units. Part or all of the modules can be selected to achieve the purpose of the present application according to actual needs. Those skilled in the art can understand and implement it without creative labor.

[0091] The embodiment of the present application also provides a computer readable storage medium, which stores a program, and the program is executed by a processor to realize the mobile robot repositioning method based on visual features and 3D laser in the above embodiment.

[0092] The computer readable storage medium can be an internal storage unit of any data processing device, such as a hard disk or a memory. The computer readable storage medium can also be an external storage device of any data processing device, such as a plug-in hard disk, a smart media card (SMC), an SD card, a flash card, etc. Further, the computer readable storage medium can include both the internal storage unit and the external storage device of any data processing device. The computer readable storage medium is used to store the computer program and other programs and data required by the data processing device, and can also be used to temporarily store data that has been output or will be output.

[0093] The above is only the preferred embodiment of the present application, and it should be pointed out that for those skilled in the art, without departing from the technical principles of the present application, a number of improvements and modifications can be made, and these improvements and modifications should also be considered as the protection scope of the present application.

Claims

1. A method for mobile robot relocalization based on visual features and 3D laser, characterized in that, The method comprises the following steps: Step 1, 3D mapping of the actual scene is carried out by using a multi-line laser radar and an IMU sensor, in the process of map construction, the pose of the mobile robot relative to the Apriltag label is obtained, the world coordinate information of the Apriltag label is obtained according to the world coordinate conversion of the mobile robot, and the label number is recorded, so as to complete the calibration of the world coordinate of the Apriltag label; Step 2, when the mobile robot loses its own positioning, the left and right cameras on the mobile robot constantly acquire the left and right images of the mobile robot, and the mobile robot stops moving when an Apriltag label is detected in the field of view of any camera; Step 3, a plurality of pose data of the mobile robot relative to the Apriltag label are obtained, denoted as set S: wherein respectively represent displacement transformation relative to the Apriltag label in the X, Y, Z axis direction, respectively represent angle transformation relative to the Apriltag label in the X, Z axis direction, represents the total number of poses obtained; Step 4, find the label world coordinate calibrated in step 1 according to the obtained Apriltag label number, denoted as , based on the relative Apriltag label pose data obtained in step 3, the world coordinate of the mobile robot is calculated, and the initial repositioning of the mobile robot is realized, and the initial repositioning result of the mobile robot is denoted as ; Specifically, the left and right cameras detect the label and obtain the relative pose data to perform preliminary repositioning, and the specific steps are as follows: 4-1, according to whether the label is detected by the left and right cameras during the actual movement of the mobile robot, the situation can be divided into two categories, one is single side, and the other is double side; single side refers to only the left camera or the right camera detecting the label, then step 4-2 is performed, and double side refers to both the left and right cameras detecting the label, then step 4-3 is performed; 4-2、In the case of single side, when multiple tags are detected within the camera range, record the detected tag number as , where m represents the number of detected tags; find the corresponding calibrated tag world coordinates according to the tag number as , take the relative displacement transformation information in the pose data of the mobile robot relative to the tag at this time , the angle information and , and obtain the preliminary world coordinates of the mobile robot as , the angle , ; 4-3、In the case of bilateral, record the left side detection of the label number as , wherein represents the number of left side detection labels, and the right side is , wherein represents the number of right side detection labels, and the corresponding label world coordinates are found according to the label number , ; take the relative displacement transformation information in the pose data of the mobile robot relative to the label at this time , , the angle information , and , , to obtain the preliminary world coordinates of the mobile robot , and the angle , ; Step 5, The coordinates are combined with multi-line laser point cloud information for further calculation. Specifically, the following steps are taken: First, voxel filtering and noise removal are performed on the current frame point cloud data of the 3D laser on the mobile robot. The space where the mobile robot is located is divided into cubic grids with fixed side lengths to form a grid space. Based on whether the points in the point cloud data obtained from the 3D laser scan are inside the cubic grid, the center value of the cubic grid is set to 1 or 0. The values ​​of the remaining points inside the cubic grid are calculated using the center values ​​of the adjacent cubic grids. The magnitude of the value represents the probability of an obstacle. Then, the coordinates are used to calculate the value of the point cloud data. Centered on parameters For the radius and in combination with step 4 Compared to The angle information is used to perform a directional range search; the current frame point cloud data is transformed in 3D at equal intervals of displacement and angle, and the probability value is obtained based on the coordinates of the transformed points in the grid space. The sum of the probability values ​​is calculated, and the pose data of the interval transformation corresponding to the maximum sum is used as the result of precise relocalization. The precise repositioning is achieved by combining multi-line lidar point cloud data matching, specifically as follows: 5-1. The world coordinates of the mobile robot obtained in step 4 is the center of the sphere, and parameter is the radius, and angle and establish a spherical cone, parameter can be set autonomously according to the actual environment; within the range of the spherical cone, a plurality of equal intervals are set according to the set angle The point cloud data obtained by the current frame scanning of the 3D laser is transformed in the 3D space, and the point cloud data before transformation is denoted as , and the point cloud data after transformation is denoted as ; 5-2, by using the grid idea, the space where the current mobile robot is located is divided into fixed-length cubes, and the value at the center of the cube represents the probability that the cube is an obstacle; the generalization formula of the probability value q is as follows: Wherein, A and B are constant coefficients, A=1 and B=0 when t=0, A=0 and B=1 when t=1, and the determination process of the constant coefficients A and B is as follows: Perform on point k After transformation, the map coordinates are obtained as follows: Query 3D occupancy raster map Corresponding grid number Calculate its grid center based on the grid number. ,by , , Taking this as an example for analysis, let's record... for ,Will Adding the resolution r yields Record it as The resolution r can be set manually; here it is set to 5cm. , , ,but ; According to and Computing normalized coordinates ; acquire the label number of the grid where the point is located , acquire the grid probability according to the label number , respectively add 1 to the label, acquire the label number of the remaining 7 points, and acquire the probability according to the transformed label number , the value is 0 or 1, and the probability q of the point is obtained according to the obtained 8 probability values By comparing the generalization formula of the probability value q with the specific form of the formula, the specific form of the constant coefficients A and B can be known, that is, the higher the probability value, the darker the cube, which represents a higher possibility of being an obstacle, and the lower the probability, the more likely the position is empty, and the cubes in the unknown area are set to gray, that is, the probability value is 0.5; 5-3、According to the different intervals set To the current scanning point cloud data Transform, and obtain the probability value according to the coordinates of the transformed points in the grid space, accumulate the probability value of the current point cloud data points, establish a least square model for nonlinear optimization, and introduce a loss function, so as to obtain the optimal transformation ; wherein K represents the number of point clouds obtained by scanning at time t, represents a loss function , used to exclude the influence of outliers on the optimization result, s represents the probability value of the cubic grid, and Q represents the calculation probability function, represents the coordinates of the kth point in the point cloud obtained by scanning at time t in the mobile robot coordinate system, represents the transformation matrix for transforming the coordinates of the kth point in the mobile robot coordinate system into the coordinates in the world coordinate system; 5-4. Based on the optimal transform , and transformed into , as the final pinpointed coordinates. 2.The method of claim 1, wherein: In step 3, the is recorded as After removing outliers from the set, the mean value is obtained by mean filtering , reducing the influence of local outliers on the overall determination result. 3.The method of claim 1, wherein: The calibration of the world coordinate of the Apriltag label in step 1 is specifically as follows: 1-1, the point where the mobile robot starts mapping is taken as the origin of the world coordinate system, in the moving process, the point cloud data obtained by the 3D laser scanning on the mobile robot is matched with the point cloud data obtained in the previous scanning process, a subgraph strategy is adopted, N frames of point cloud data are used to establish a subgraph, and nodes are constructed according to the pose of the mobile robot every N frames, in the process of continuous movement of the mobile robot, the transformation process constraint between nodes and the constraint between the current frame and the adjacent node are continuously calculated, the constraint is taken as the edge between nodes, and nonlinear optimization is performed, which is converted into a graph optimization problem, and the optimization function is constructed as follows: In the above formula, , respectively represent a subgraph set with a constraint relationship of the current scanning frame and a pose set of the acquired scanning frame, , respectively represent a subgraph with a constraint relationship with the current scanning frame and the number of scanning frames before the current scanning frame, , respectively represent the relative pose and the covariance matrix of the scanning frame j relative to the subgraph i, , represents a nonlinear error function about , , , , the optimal subgraph pose and the optimal scanning frame pose are solved by using the Levenberg-Marquardt algorithm, and are converted into the mobile robot world coordinates at the current time in combination with the initial pose of the mobile robot. 1-2, in the process of constructing the map, the left and right cameras constantly acquire images for detection, when an Apriltag label is detected, 0 and 1 are used to distinguish the data recorded by the left and right cameras, the label number detected at this moment is recorded, and the world coordinate of the Apriltag label is obtained according to the world coordinate conversion of the mobile robot and is saved for subsequent use.

4. A mobile robot relocalization device based on visual features and 3D laser, comprising a memory and one or more processors, wherein the memory stores executable code, and the executable code comprises the following steps: When the processor executes the executable code, the mobile robot repositioning method based on visual features and 3D laser according to any one of claims 1-3 is realized. ​ 5. A computer-readable storage medium having stored thereon a program, characterized in that, The program is executed by the processor to implement the mobile robot repositioning method based on visual features and 3D laser as claimed in any one of claims 1-3.

Images