Localization method for household lawn mowing robot based on enhanced loop detection and relocalization

Through the visually assisted laser SLAM method of enhanced loop detection and repositioning, combined with the Cartographer algorithm and BOW model, the problem of low robustness and long-term mapping of traditional mowing robot positioning systems is solved, and more accurate navigation and environmental perception is achieved .

CN114092714BActive Publication Date: 2024-11-01JIANGSU UNIV OF SCI & TECH
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202111376344.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-11-19
Publication Date
2024-11-01
Estimated Expiration
2041-11-19

AI Technical Summary

Technical Problem

The positioning system of traditional mowing robots is low in robustness and is easily disturbed by environmental interference, resulting in inaccurate navigation and long time to build maps, and the closed-loop detection module is slow in detection speed and small detection range.

Method used

Using the visually assisted laser SLAM method based on enhanced loop detection and relocation, data is collected through laser data, IMU data and camera, combined with the Cartographer algorithm, voxel filtering and Bag of Words (BOW) model are used to perform closed-loop detection and relocation, to improve the Map construction robustness and accuracy.

Benefits of technology

It effectively improves the positioning accuracy and robustness of the mowing robot, shortens the map construction time, improves the environment perception ability, and significantly improves navigation performance.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114092714B_ABST
    Figure CN114092714B_ABST
Patent Text Reader

Abstract

The present invention discloses a positioning method for a household mowing robot based on enhanced loop detection and repositioning, including: collecting and reading sensor data; obtaining robot posture information; transferring the robot posture information and processed laser data into the local SLAM part in the Cartographer algorithm to obtain the insertion result; receiving the collected image information, extracting the feature points of the image, describing the feature points, that is, calculating the feature descriptor, according to the feature descriptor, querying the dictionary to calculate the BOW vector of the frame, and transmitting the BOW vector and the image index to the global SLAM part in the Cartographer algorithm; in the global SLAM, the image index and the current node id are bundled together, recorded as node_to_image, and the database is updated. The current laser data is used at the posture of the node to match the map at the location, and the map is completed. The present invention uses visual assistance Cartographer back-end optimization to effectively improve the robustness of the map, so that the map is more accurate, and the robot can be more accurately positioned and navigated.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of lawn mowing robots and relates to a vision-assisted laser SLAM method, and in particular to a positioning method for a household lawn mowing robot based on enhanced loop detection and repositioning. Background Art

[0002] Traditional lawn mowers require manual assistance to complete mowing. The disadvantages of manual mowing include monotony, time-consuming, obstacles, gravel, harmful mosquitoes in the grass, etc. However, the emergence of lawn mowing robots has solved the above problems and brought convenience to people. The accurate positioning of lawn mowing robots is one of the keys to ensure that they can correctly complete navigation and control tasks. The accuracy of the lawn mowing robot walking along the planned path mainly depends on the positioning system. Therefore, the lawn mowing robot must obtain instant and high-precision posture information through the positioning system equipment, so as to decide the next movement state. The traditional positioning mode uses GNSS positioning, which has low robustness and is easily affected by environmental interference.

[0003] Therefore, a new technical solution is needed to solve this problem. Summary of the invention

[0004] Purpose of the invention: In order to overcome the problems of slow detection speed and small detection range of the closed-loop detection module in the original Cartographer, a positioning method for a household lawn mowing robot based on enhanced loop detection and relocalization is provided. It uses vision to assist the back-end optimization of Cartographer, effectively improves the robustness of map construction, makes the map more accurate, and enables the robot to locate and navigate more accurately.

[0005] Technical solution: To achieve the above-mentioned purpose, the present invention provides a positioning method for a household lawn mowing robot based on enhanced loop detection and re-positioning, comprising the following steps:

[0006] S1: Collect and read sensor data, including laser data, IMU data and camera data;

[0007] S2: Fuse odometer data and IMU data to obtain robot posture information and perform voxel filtering on laser data;

[0008] S3: Pass the robot pose information and processed laser data into the local SLAM part of the Cartographer algorithm to obtain the insertion result;

[0009] S4: Receive the collected image information, extract the feature points of the image, describe the feature points, that is, calculate the feature descriptor, query the dictionary based on the feature descriptor to calculate the BOW vector of the frame, and transmit the BOW vector and image index to the global SLAM part of the Cartographer algorithm;

[0010] S5: In global SLAM, the image index and the current node id are bundled together, recorded as node_to_image, and the database is updated. The corresponding node index is found according to the image index, and the current laser data is used at the position of the node to match the map at that location to complete the mapping.

[0011] Furthermore, the laser data in step S1 includes the distance value of the obstacle detected by the laser radar, the odometer data includes the movement speed of the robot, and the IMU data includes the acceleration and angular velocity collected by the IMU.

[0012] Furthermore, the robot posture information is obtained in step S2 in the following manner:

[0013] The acceleration collected by the IMU is integrated to calculate the velocity and displacement. The pose inference device in the Cartographer algorithm is used to multiply the linear velocity calculated by the two data at the beginning and end of the pose data queue by the time to predict the translation of the lawn mowing robot. The pose prediction is obtained by multiplying the angular velocity of the imu by the time to obtain the robot's pose information.

[0014] Furthermore, the specific process of obtaining the insertion result in step S3 is as follows:

[0015] Scan-match correlation scanning matching technology is used to calculate the relative pose of adjacent laser points. If the initial pose is known, the global absolute pose of all scan nodes can be obtained. Due to its principle, cumulative errors will be introduced, but since the relative pose is relatively accurate, the accumulated error is small in a short time. Cartographer uses scans within a period of time to construct submaps instead of the entire map; each submap is independent of each other, and the scan-to-map idea is used for scan-match to further reduce the cumulative error, and finally outputs submaps, poses, scan data, etc.

[0016] Furthermore, the feature points of the image in step S4 are extracted in the following manner:

[0017] Based on the image pyramid method, OrientedFAST key points (FAST key points with added orientation) and BRIEF descriptors are extracted from images of different scales to achieve scale and orientation invariance.

[0018] Furthermore, in step S4, a brief descriptor is used to describe the feature points. The brief descriptor is a binary descriptor with a total length of 256 bits, i.e., 32 bytes. Each bit is 0 or 1. Point pairs are selected according to a certain point pair selection rule. The selection rule should minimize the correlation between point pairs.

[0019] Furthermore, the BOW vector in step S4 is specifically:

[0020] BOW, Bag of Words, bag of words model. Let's assume that a word is equivalent to a feature point. It is the cluster center of several feature points. Of course, it is still a feature point, but several close feature points are mapped to the same feature point, giving it a new name called word. Then a frame of image, several feature points, can be mapped to several words, and the word set is BOW.

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

[0022] A1: The image index and the current node id are bundled together, recorded as node_to_image, and the "word-image index" database is updated, recorded as KeyFrameDataBase, which stores the index of the image where each word is located;

[0023] A2: Query closed loop (computational constraints): Based on the word in the current image, query all historical images in the database that contain any of the same words. After finding the image, find the corresponding node index based on the image index, and then use the current laser data at the position of the node to match it with the map at that location;

[0024] A3: Save the adjusted pose and map, and save the database to the hard disk.

[0025] The present invention uses a laser SLAM algorithm to realize the positioning and navigation of a lawn mowing robot, which can realize real-time positioning of the lawn mowing robot and improve its robustness. In the laser SLAM algorithm, the Cartographer algorithm based on graph optimization is selected to realize accurate mapping of the lawn environment. In view of the serious time-consuming optimization and repositioning of the Cartographer backend and the problem that closed-loop failures are prone to occur, a visually assisted closed-loop detection and repositioning method based on a bag-of-words library (BOW) is proposed.

[0026] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0027] 1. This invention overcomes the problems of traditional Cartographer in outdoor lawn mapping, such as long time consumption and low robustness. Experiments show that the observation area, robustness and fault tolerance of map creation are effectively improved.

[0028] 2. The visually assisted closed-loop detection and relocalization method based on bag-of-words (BOW) proposed in the present invention enables the lawn mowing robot to have more accurate mapping and positioning capabilities during navigation.

[0029] 3. The present invention utilizes a binocular camera to obtain three-dimensional environmental information, effectively improving the environmental perception capability of the lawn mowing robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is the overall algorithm flow chart of the present invention;

[0031] Figure 2 Create an environment map of the teaching building lawn for Gazebo;

[0032] Figure 3 2D environment maps for Cartographer;

[0033] Figure 4 This is the time-consuming graph before improvement;

[0034] Figure 5 This is the time-consuming diagram after improvement;

[0035] Figure 6 The following is a comparison chart of the time consumed before and after the improvement. DETAILED DESCRIPTION

[0036] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, various equivalent forms of modifications to the present invention by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0037] The present invention provides a positioning method for a household lawn mowing robot based on enhanced loop detection and re-positioning, such as Figure 1 As shown, it includes the following steps:

[0038] S1: Collect and read sensor data, including laser data, IMU data, and camera data:

[0039] The industrial computer reads the acceleration and angular velocity collected by the IMU, the left and right images collected by the binocular camera, the distance value collected by the 360° two-dimensional laser radar, and the robot movement speed collected by the encoder in real time through the USB interface.

[0040] S2: Integrate the robot's motion speed and IMU data to obtain the robot's posture information and perform voxel filtering on the laser data:

[0041] In this embodiment, the method for obtaining the robot posture information is specifically as follows:

[0042] The acceleration collected by the IMU is integrated to calculate the velocity and displacement. The pose inference device in the Cartographer algorithm is used to multiply the linear velocity calculated by the two data at the beginning and end of the pose data queue by the time to predict the translation of the lawn mowing robot. The pose prediction is obtained by multiplying the angular velocity of the imu by the time to obtain the robot's pose information.

[0043] S3: Pass the robot pose information and processed laser data to the local SLAM part of the Cartographer algorithm to obtain the insertion result:

[0044] The specific process of obtaining the insertion result is:

[0045] Scan-match correlation scanning matching technology is used to calculate the relative pose of adjacent laser points. If the initial pose is known, the global absolute pose of all scan nodes can be obtained. Due to its principle, cumulative errors will be introduced, but since the relative pose is relatively accurate, the accumulated error is small in a short time. Cartographer uses scans within a period of time to construct submaps instead of the entire map; each submap is independent of each other, and the scan-to-map idea is used for scan-match to further reduce the cumulative error, and finally outputs submaps, poses, scan data, etc.

[0046] S4: Receive the collected image information, extract the feature points of the image, describe the feature points, that is, calculate the feature descriptor, query the dictionary based on the feature descriptor to calculate the BOW vector of the frame, and transmit the BOW vector and image index to the global SLAM part of the Cartographer algorithm:

[0047] The feature points of the image are extracted as follows:

[0048] Based on the image pyramid method, OrientedFAST key points (FAST key points with added orientation) and BRIEF descriptors are extracted from images of different scales to achieve scale and orientation invariance.

[0049] The brief descriptor is used to describe the feature points. The brief descriptor is a binary descriptor with a total length of 256 bits, that is, 32 bytes. Each bit is 0 or 1. Point pairs are selected according to certain point pair selection rules. The selection rules should minimize the correlation between point pairs.

[0050] The BOW vector is specifically:

[0051] BOW, Bag of Words, bag of words model. Let's assume that a word is equivalent to a feature point. It is the cluster center of several feature points. Of course, it is still a feature point, but several close feature points are mapped to the same feature point, giving it a new name called word. Then a frame of image, several feature points, can be mapped to several words, and the word set is BOW.

[0052] S5: In global SLAM, the image index and the current node id are bundled together, recorded as node_to_image, and the database is updated. The corresponding node index is found according to the image index, and the current laser data is used at the position of the node to match the map at that location to complete the mapping.

[0053] Step S5 specifically includes the following process:

[0054] A1: The image index and the current node id are bundled together, recorded as node_to_image, and the "word-image index" database is updated, recorded as KeyFrameDataBase, which stores the index of the image where each word is located;

[0055] A2: Query closed loop (computational constraints): Based on the word in the current image, query all historical images in the database that contain any of the same words. After finding the image, find the corresponding node index based on the image index, and then use the current laser data at the position of the node to match it with the map at that location;

[0056] A3: Save the adjusted pose and map, and save the database to the hard disk.

[0057] Based on the above scheme, in order to verify the effectiveness and practical effect of the method of the present invention, the following simulation experiments are carried out:

[0058] like Figure 2As shown, a simulation environment is established in Gazebo. The simulation environment includes flowers, trees and rockery with different heights. Figure 1 represents a white rose bush, which is lower than the installation height of the single-line laser radar, while Figure 7 represents a bush of flowers and trees (flowers are planted around the trees). The blue part in Figure 7 represents a bush of flowers, and the red and yellow parts represent trees. The height of the trees is higher than the set height of the single-line laser radar, but the bush of flowers is lower than the set height of the single-line laser radar. Figure 6 represents a lawn mowing robot, and Figures 3, 4, and 5 represent other types of flowers and their heights are higher than the laser radar height. Figure 2 is a rockery, and the rest are trees. rviz is a graphical tool that comes with ROS. Figure 7 is a two-dimensional environment map created based on the lawn of the teaching building in Figure 6 using RVIZ (graphical tool) and the Cartographer algorithm, as shown in Figure 6. Figure 3 shown.

[0059] A quantitative analysis of the Cartographer algorithm before and after the improvement (the red font indicates the improved method) is performed. A total of 20 pose points (green points) are selected from the starting point to the end point of the map to conduct a relocalization experiment and the time consumption is counted. It can be seen that Figure 4 As the order of postures increases, the time required for relocalization gradually increases. When approaching the end point, the time required for relocalization is as long as about 300 seconds. Figure 5 It can be seen that the longest time consumption is only about 1.6s. Figure 6 This is a time-consuming comparison chart, which shows that its robustness has been significantly improved.

Claims

1. Localization method of household lawn mowing robot based on enhanced loop detection and relocalization, It is characterized in that The steps include: S1: Collect and read sensor data, including laser data, IMU data and camera data; S2: Fuse odometer data and IMU data to obtain robot posture information and perform voxel filtering on laser data; S3: Pass the robot pose information and processed laser data into the local SLAM part of the Cartographer algorithm to obtain the insertion result; S4: Receive the collected image information, extract the feature points of the image, describe the feature points, that is, calculate the feature descriptor, query the dictionary based on the feature descriptor to calculate the BOW vector of the frame, and transmit the BOW vector and image index to the global SLAM part of the Cartographer algorithm; S5: In global SLAM, the image index and the current node id are bundled together, recorded as node_to_image, and the database is updated. The corresponding node index is found according to the image index, and the current laser data is used at the position of the node to match the map at that location to complete the mapping.

2. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that The laser data in step S1 includes the distance value of the obstacle detected by the laser radar, the odometer data includes the movement speed of the robot, and the IMU data includes the acceleration and angular velocity collected by the IMU.

3. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 2, It is characterized in that The method for acquiring the robot posture information in step S2 is specifically as follows: The acceleration collected by the IMU is integrated to calculate the velocity and displacement. The pose inference device in the Cartographer algorithm is used to multiply the linear velocity calculated by the two data at the beginning and end of the pose data queue by the time to predict the translation of the lawn mowing robot. The pose prediction is obtained by multiplying the angular velocity of the imu by the time to obtain the robot's pose information.

4. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that The specific process of obtaining the insertion result in step S3 is as follows: The scan-match correlation scan matching technology is used to calculate the relative poses of adjacent laser points. If the initial pose is known, the global absolute pose of all scan nodes can be obtained. Cartographer uses scans within a set time to construct submaps. Each submap is independent of each other, and the scan-to-map idea is used for scan-match to further reduce the cumulative error. Finally, the submap, pose and scan data are output.

5. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that The method for extracting the feature points of the image in step S4 is: Based on the image pyramid method, Oriented FAST key points and BRIEF descriptors are extracted from images of different scales to achieve scale and direction invariance.

6. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that In step S4, a brief descriptor is used to describe the feature points. The brief descriptor is a binary descriptor with a total length of 256 bits, i.e., 32 bytes. Each bit is 0 or 1. Point pairs are selected according to a point pair selection rule. The selection rule should minimize the correlation between point pairs.

7. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that The BOW vector in step S4 is specifically: A word is equivalent to a feature point, which is the clustering center of several feature points. A frame of image and several feature points can be mapped to several words, and the word set is BOW.

8. The positioning method of a household lawn mowing robot based on enhanced loop detection and relocalization according to claim 1, It is characterized in that The step S5 specifically includes the following process: A1: The image index and the current node id are bundled together, recorded as node_to_image, and the "word-image index" database is updated, recorded as KeyFrameDataBase, which stores the index of the image where each word is located; A2: Query closed loop: Based on the word in the current image, query all historical images in the database that contain any of the same words. After finding the image, find the corresponding node index based on the image index, and then use the current laser data at the position of the node to match it with the map at that location; A3: Save the adjusted pose and map, and save the database to the hard disk.