Indoor robot positioning and mapping method based on lightweight multi-sensor fusion

Abstract

The present application relates to the technical field of indoor robot navigation, and particularly relates to a positioning and mapping method of an indoor robot based on lightweight multi-sensor fusion. The method comprises the following steps: collecting wheel speed encoder data, laser sensor data and two-dimensional code camera data, and preprocessing the collected data; processing the laser point cloud data by using wheel speed odometry information, solving the robot pose by using an error iterative Kalman filtering algorithm combined with the processed laser point cloud data, and correcting the robot pose by using the position information of the two-dimensional code; maintaining and updating the laser point cloud map by using a voxel grid method, converting the laser point cloud data into the map coordinate system, and inserting the laser point cloud data into the corresponding voxel grid; the system synchronously solves the pose and updates the map structure in the mapping mode, and loads the existing map and solves the pose by using the two-dimensional code with a known position in the positioning mode. The technical scheme can improve the navigation performance of the robot in a complex indoor environment, and can be deployed in a lightweight manner.

Description

Technical Field

[0001] This invention relates to the field of indoor robot navigation technology, specifically to a method for localization and mapping of indoor robots based on lightweight multi-sensor fusion. Background Technology

[0002] With the rapid development of robotics technology, mobile robots are increasingly being used in indoor scenarios such as warehousing and logistics, unmanned delivery, and intelligent inspection. Simultaneous Localization and Mapping (SLAM) is a core technology for mobile robots to achieve autonomous navigation. Its main task is to use the sensor data it carries to build an environmental map in real time and determine its own position on the map in an unknown environment.

[0003] In the field of indoor robotics, commonly used SLAM technologies mainly include wheel speed odometry, 2D laser SLAM, visual SLAM, and QR code-based absolute positioning.

[0004] Wheel speed odometry calculates the robot's displacement and heading angle through encoders. It is characterized by its simplicity and high short-term accuracy, and is often used as the front-end prediction in laser SLAM or for distortion correction of laser point clouds. 2D laser SLAM (such as algorithms based on Gmapping, Cartographer, or Hector) uses single-line or multi-line LiDAR to scan the surrounding environment to acquire point cloud data. It then estimates the robot's pose and constructs a grid map through point cloud matching (such as ICP and NDT algorithms) or graph optimization methods. Due to the high ranging accuracy of LiDAR and its minimal susceptibility to lighting conditions, 2D laser SLAM has become the most mainstream solution in industry.

[0005] However, 2D laser SLAM has significant technical limitations in practical applications: In scenarios with simple geometric features, such as long corridors and empty warehouses, the lack of significant geometric features such as corners and edges leads to insufficient constraints on point cloud matching, which can easily cause pose estimation to diverge or "get lost".

[0006] Wheel speed odometers can accumulate large errors when wheels slip or the ground is uneven, resulting in excessive initial pose deviations for laser matching. Laser algorithms often struggle to correct these large deviations in a short time, leading to positioning drift.

[0007] To improve accuracy, traditional laser SLAM typically employs graph optimization-based backend solution methods or particle filtering (such as particle filter SLAM). These algorithms require a large amount of memory and computing power, making it difficult to run smoothly on low-cost, low-power MCUs or embedded platforms.

[0008] When the environment changes (such as the movement of goods or people), the old map does not match the actual environment, which will lead to point cloud matching failure. In addition, the traditional point cloud map maintenance mechanism has huge storage and update costs in large-scale scenarios.

[0009] QR code positioning uses a camera to identify QR code labels laid on the ground, and parses the stored absolute coordinate information (ID and coordinates) to obtain a high-precision absolute pose. This method has no cumulative error and is highly accurate in positioning.

[0010] However, QR code positioning technology also has significant limitations: QR codes can only provide discrete location points (deployment intervals are typically a few meters to tens of meters), and cannot provide continuous pose estimation during movement. The robot's motion state between two QR codes must be estimated using other sensors (such as odometry).

[0011] QR code labels are easily obscured by dust, covered by stains, or physically worn, resulting in a high camera decoding failure rate and poor system robustness.

[0012] The camera needs to be actively aligned with the QR code for recognition. In high-speed movement or complex paths, there may be issues such as missed scans or recognition delays.

[0013] To address the shortcomings of single sensors, several fusion solutions have emerged in the prior art, such as fusing wheel speedometers with lasers, or fusing vision with lasers. However, existing fusion solutions generally suffer from the following problems: Most fusion algorithms (such as tightly coupled nonlinear optimization) are computationally intensive and not suitable for lightweight deployment.

[0014] Although QR codes are introduced as absolute position references, they are generally only used as nodes for closed-loop detection. The high-precision absolute coordinates of QR codes are not fully utilized to correct the drift of laser odometry in real time with strong constraints, especially in scenarios with missing features.

[0015] Traditional point cloud maps or raster maps consume a lot of memory in large-scale indoor environments, and it is difficult to quickly reset pose using known absolute position information in positioning mode. Summary of the Invention

[0016] The purpose of this invention is to propose a localization and mapping method for indoor robots based on lightweight multi-sensor fusion. This technical solution can improve the navigation performance of robots in complex indoor environments and can be deployed in a lightweight manner.

[0017] To achieve the above objectives, this invention proposes a localization and mapping method for indoor robots based on lightweight multi-sensor fusion, comprising: Collect data from wheel speed encoders, laser sensors, and QR code cameras, and preprocess the collected data; The robot pose is solved by processing the laser point cloud using wheel speed odometer information, and the robot pose is solved by combining the processed laser point cloud data with the error iterative Kalman filter algorithm. The robot pose is then corrected using the position information of the QR code. The laser point cloud map is maintained and updated using the voxel grid method, which converts the laser point cloud data to the map coordinate system and inserts it into the corresponding voxel grid. The system supports mapping mode and positioning mode. In mapping mode, pose calculation and map structure update are performed simultaneously. In positioning mode, an existing map is loaded and pose calculation is performed using QR codes at known locations.

[0018] Beneficial effects of the basic solution: The system innovatively integrates data from three sensors: a wheel speed encoder, a single-line laser sensor, and a QR code camera. This fully leverages the advantages of each sensor while compensating for the shortcomings of a single sensor, achieving a dual improvement in positioning and mapping accuracy and stability. The wheel speed encoder provides real-time basic odometer information for robot motion, offering an initial reference for pose determination; the laser sensor quickly collects spatial features of the indoor environment, capturing environmental contours through point cloud processing, providing core spatial data for pose determination and map construction; and the QR code camera provides precise absolute position information, effectively correcting the positioning errors accumulated by the laser odometry and wheel speed odometry, preventing error drift.

[0019] The synergistic effect of these three components enables the system to maintain stable and reliable positioning performance even in complex indoor scenarios. This not only solves the problem of error accumulation caused by the susceptibility of a single wheel speed odometer to ground friction and slippage, but also compensates for the shortcomings of laser sensors in open environments, such as insufficient feature points and inaccurate positioning. Furthermore, the absolute positioning calibration via QR codes further improves the accuracy of pose calculation, ensuring that the robot can maintain high positioning accuracy during long-term operation and providing a solid foundation for subsequent map building and path planning.

[0020] The system overcomes the limitations of traditional positioning and mapping systems by employing a multi-sensor fusion strategy and flexible operating modes. It is widely adaptable to various indoor scenarios, particularly suitable for open, changeable, highly dynamic, and confined spaces—areas where traditional systems struggle. In mapping mode, the system simultaneously performs pose calculation and map structure updates, enabling real-time capture of environmental changes and timely map data updates. In positioning mode, the system loads an existing map and combines it with QR codes at known locations for pose calculation. Even with localized environmental changes, accurate positioning can be achieved through the fusion of laser point clouds and QR codes.

[0021] Compared to single-sensor systems, this system does not rely on fixed environmental features. Whether in a spacious, unobstructed hall, a dynamically changing office area, or a narrow corridor, it can stably complete localization and mapping tasks, greatly expanding the application scenarios of indoor robots and improving the practicality and applicability of the system.

[0022] The system uses the iterative Kalman filter algorithm as its core, combined with the iVox voxel grid map update method, to construct an efficient and lightweight localization and mapping algorithm framework. This effectively solves the problems of high computational requirements and difficulty in running traditional laser SLAM algorithms on low-cost platforms. The Kalman filter algorithm mainly uses matrix operations and does not involve complex optimization calculations, resulting in high computational efficiency and low resource consumption. Combined with the iVox voxel grid method for maintaining and updating the laser point cloud map, it not only simplifies the map update process and improves the update speed, but also further reduces computational complexity.

[0023] This allows the system to operate stably on low-computing-power platforms at the MCU level without relying on high-performance processors, and without the need for complex software libraries. This significantly reduces the system's demand for computing resources, breaks the limitation that high-precision positioning and mapping must rely on high-computing-power platforms, and provides technical support for the research and development and application of low-cost indoor robots.

[0024] The system supports two working modes: mapping mode and localization mode, which can be flexibly switched according to the actual application scenario to meet the different working needs of the robot. In mapping mode, the system can complete pose calculation and map building in real time, realizing simultaneous movement, mapping, and localization, which is suitable for scenarios where the robot enters an unknown indoor environment for the first time. In localization mode, the system can directly load an existing map and quickly complete pose calculation by combining it with a QR code, without the need to rebuild the map, thus improving the robot's working efficiency and making it suitable for tasks such as inspection and navigation in known environments.

[0025] Meanwhile, the point cloud map can still be dynamically updated in positioning mode, which can adapt to dynamic changes in the environment, further improving the system's flexibility and practicality, allowing the robot to better adapt to the complex needs in practical applications, and improving work efficiency and user experience.

[0026] The core innovation of the system lies in proposing a 2D laser SLAM algorithm based on iterative Kalman filtering and a localization and mapping framework that fuses QR codes and lasers, forming a protectable core technology. Specifically, the iterative Kalman filtering algorithm, designed for single-line lasers, constructs residuals based on point-to-line distances, enabling real-time and accurate robot pose calculation. Combined with the iVox voxel grid method to maintain the point cloud map, it offers faster and more lightweight localization and mapping compared to existing 2D laser SLAM technologies. The overall algorithm framework for fusing QR codes and lasers achieves seamless switching and complementary advantages between the two working modes, solving the localization and mapping challenges in easily changing scenarios.

[0027] As a feasible and preferred solution, the acquisition and preprocessing of wheel speed encoder data specifically includes: Configure the encoder's resolution and counting mode; The encoder pulse count is read in each sampling period to calculate the wheel rotation distance; The linear velocity of the wheel is calculated based on the rotation distance and sampling time; Based on the dual-wheel differential drive model, the robot's pose change is estimated using the motion distance of the left and right wheels. The pose update formula is:

[0028] in, For position coordinates, Yaw angle and These represent the travel distances of the left and right wheels, respectively. Given the wheelbase, the coordinate transformation matrix obtained from the pose is: .

[0029] As a feasible and preferred approach, the acquisition and preprocessing of laser sensor data specifically includes: Configure the scanning frequency and resolution of the LiDAR; The raw data from the LiDAR is read in each scanning cycle and the point cloud coordinates are obtained by parsing. The point cloud data is filtered to remove noise points and outliers.

[0030] As a feasible and preferred solution, QR code camera data acquisition and preprocessing specifically include: Configure the camera's resolution and frame rate; Read camera image data in each sampling period; The QR code recognition algorithm is used to parse the QR code information in the image to obtain the unique ID and coordinate information of the QR code; and Convert the relative coordinates of the QR code to absolute coordinates in the robot's coordinate system.

[0031] As a feasible and preferred approach, processing the laser point cloud using wheel speed odometer information specifically refers to distortion correction, including: By utilizing the high-frequency pose information from the wheel speed odometer, the pose at any given time can be obtained through linear interpolation. For each laser point, its coordinates are transformed to the same moment based on the pose obtained by interpolation, thus achieving motion distortion correction. Previous moment up to the current moment Using high-frequency pose data from wheel speed odometers: ...through linear interpolation, we obtain Given the odometry pose at any given time, we then perform coordinate transformation on each point of the laser beam, using the following formula:

[0032] Transform all point cloud coordinates to At any given moment, motion distortion can be corrected.

[0033] As a feasible and preferred approach, solving the robot pose using processed laser point cloud data specifically includes: Initial pose is provided by wheel speed-odometer:

[0034] The error between the true pose and the estimated pose is defined as:

[0035] The wheel speed odometer update formula is written as:

[0036] right The linear expansion is as follows:

[0037] for right The Jacobian matrix of the derivative. For noise, obey Gaussian release , It is also a Jacobian matrix.

[0038] As a feasible and preferred approach, the iterative solution process includes: For the j-th iteration, construct the residual model: for each distortion-corrected map point... Convert to map coordinate system Use the kd-tree method to find the two closest points on the map. and Based on the distance from the point to the line, construct the residual:

[0039] Iterative Kalman Calculation: Residual The linearized expansion is as follows:

[0040] in, for right The Jacobian matrix of the derivative. To measure noise, conform to Gaussian distribution. ; Calculate the covariance matrix:

[0041] Calculate the Kalman gain:

[0042] in:

[0043]

[0044] Iterative pose update:

[0045] When increment The iteration ends when the value is sufficiently small or the maximum number of iterations is reached. Update pose and covariance: , .

[0046] As a feasible and preferred solution, using the location information of QR codes to correct robot pose specifically includes: QR code labels with unique IDs and coordinate information are placed on the ground. When a robot passes by, it uses a camera to scan the QR code to obtain the landmark information.

[0047] in, The xy coordinates and yaw angles of the QR code with ID i relative to the robot; Redefine state variables:

[0048] in, The map pose of the QR code with ID i; Establish the relationship between the QR code's pose on the map and the robot's pose: ; Upon first recognition of the QR code, the robot initializes its state based on the QR code's landmark information and its current pose. The state transition equation is derived from the wheel speed odometer update formula as follows:

[0049] Will and They are respectively for and noise The derivative of the Jacobian matrix linearly expands the measurement equation: ; The machine pose and the pose of each QR code are updated based on Kalman filtering: Predicting covariance: ; Calculate the Kalman gain: ; Update pose and covariance: , .

[0050] As a feasible and preferred solution, the mapping mode specifically includes: Laser positioning and map update: The laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated. QR code positioning correction: When a QR code is recognized, the robot's pose is corrected based on the landmark information in the QR code.

[0051] As a feasible and preferred solution, the positioning mode specifically includes: Loading the map: The robot loads the pre-built map, recognizes a QR code in the map, and obtains the robot's initial pose on the map based on the road sign information in the QR code and the map pose. Laser positioning and map updating: During machine movement, the laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated. QR code positioning correction: When the machine recognizes the QR code, since the location of the QR code on the map is already determined, the pose is corrected based on the QR code. State variables and measurement equations are defined, and the pose is corrected based on the Kalman filter principle. Attached Figure Description

[0052] Figure 1 This is a schematic diagram of the system hardware architecture.

[0053] Figure 2 This is a schematic diagram of the overall process of this embodiment.

[0054] Figure 3 This is a schematic diagram of the fusion of odometer readings and laser point cloud data. Detailed Implementation

[0055] To make the technical solution and advantages of this application clearer, the technical solution of the present invention will be further described in detail below with reference to the accompanying drawings. It is understood that the specific embodiments described herein are only some embodiments of the present invention, and are only used to explain this application, not to limit it. It should be noted that the technical features or combinations of technical features described in the following embodiments should not be considered isolated; they can be combined with each other to achieve better technical effects. The same reference numerals appearing in the accompanying drawings of the following embodiments represent the same features or components, and can be applied to different embodiments.

[0056] Furthermore, unless otherwise defined, the technical or scientific terms used in this invention description shall have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains.

[0057] The present invention will now be described in further detail with reference to the accompanying drawings.

[0058] This disclosure provides a localization and mapping system for an indoor robot based on lightweight multi-sensor fusion, referring to... Figure 1 The hardware utilizes the commonly used STM32F407 MCU platform, with external RAM and FLASH to expand memory and storage capacity. A single-line laser, a QR code camera, and a wheel speed encoder are connected via serial port and mounted on a dual-wheel differential chassis. The STM32F407 runs on the real-time operating system FreeRTOS to implement and execute the aforementioned positioning and mapping algorithms. The results show that with a data frequency of 10Hz for the laser radar and QR code camera, and 50Hz for the wheel speed encoder, the positioning accuracy reaches 0.1~0.2m within a 5cm range error of the laser radar.

[0059] The system mainly consists of a sensor data acquisition module, a data processing and fusion module, a positioning and mapping module, and a communication and control module. Each module interacts with the other via the MCU's GPIO, UART, and SPI interfaces.

[0060] The system mainly includes: Wheel speed encoder module, used to collect wheel rotation data; A laser sensor module is used to acquire laser point cloud data; The QR code camera module is used to acquire QR code image data; The multi-sensor fusion module is used to perform distortion correction on the laser point cloud using wheel speed odometer information. Based on the iterative Kalman filter algorithm, the distortion-corrected laser point cloud data and the absolute position information of the QR code are fused to solve the robot pose. The map maintenance module is used to maintain laser point cloud maps using a voxel grid method; and The mode switching module supports switching between mapping mode and localization mode. In mapping mode, a map is built and the QR code pose is estimated. In localization mode, an existing map is loaded and the robot pose is corrected using QR codes at known locations.

[0061] The multi-sensor fusion module mainly includes: The distortion correction unit is used to perform motion distortion correction on the laser point cloud by linear interpolation using high-frequency pose information from the wheel speed odometer. The pose calculation unit is used to perform iterative pose calculation based on the point-to-line distance residual using an error iterative Kalman filter algorithm; and The QR code fusion unit is used to jointly estimate the robot's pose and the QR code's pose in mapping mode, and to correct the robot's pose using the QR code at a known location in localization mode.

[0062] This disclosure also provides a method for localization and mapping of an indoor robot based on lightweight multi-sensor fusion, including the following steps, referred to... Figure 2 .

[0063] Step S100, sensor data acquisition and preprocessing, includes: Step S101: Wheel speed encoder data acquisition. The wheel speed encoder estimates the robot's position, speed, and angle by measuring the rotation angle and speed of the wheels. The specific steps are as follows: Encoder initialization: Configure the encoder's resolution and counting mode.

[0064] Pulse counting: Read the pulse count of the encoder in each sampling period and calculate the rotation distance of the wheel; Speed ​​estimation: Calculate the linear velocity of the wheel based on the rotation distance and sampling time.

[0065] Pose update: Based on the dual-wheel differential drive model, the robot's pose change is estimated by using the speed of the left and right wheels.

[0066] The pose update formula is:

[0067] in, For position coordinates, Yaw angle and These represent the travel distances of the left and right wheels, respectively. Let the wheelbase be denoted as . The coordinate transformation matrix obtained from the pose is: .

[0068] Step S102: Laser sensor data acquisition. The laser sensor acquires point cloud data by scanning the surrounding environment. The specific steps are as follows: LiDAR initialization: Configure the LiDAR's scanning frequency and resolution; Point cloud data acquisition: Read the raw data from the LiDAR in each scanning cycle and parse it to obtain the point cloud coordinates; Data filtering: Filter the point cloud data to remove noise points and outliers.

[0069] Step S103: QR code camera data acquisition. The QR code camera captures images of QR code labels on the ground and parses their stored unique IDs and coordinate information. The specific steps are as follows: Camera initialization: Configure the camera's resolution and frame rate; Image acquisition: Read the camera's image data in each sampling period; QR code recognition: Use a QR code recognition algorithm (such as ZBar or OpenCV's QRCodeDetector) to parse the QR code information in the image; Coordinate transformation: Convert the relative coordinates of the QR code to absolute coordinates in the robot's coordinate system.

[0070] Step S200, multi-sensor data fusion and localization, includes: Step S201, laser point cloud distortion correction: During robot movement, the point cloud scanned by the laser sensor will exhibit motion distortion, requiring distortion correction processing. (Refer to...) Figure 3 The specific steps are as follows: Pose interpolation: Using high-frequency pose information from wheel speed odometers, the pose at any time is obtained through linear interpolation; Coordinate transformation: For each laser point, its coordinates are transformed to the same time based on the pose obtained by interpolation, thus achieving motion distortion removal.

[0071] During robot movement, the point cloud scanned by the laser sensor will exhibit motion distortion. (The last sentence appears to be incomplete and lacks context.) up to the current moment Using high-frequency pose data from wheel speed odometers: ...through linear interpolation, we obtain Given the odometry pose at any given time, we then perform coordinate transformation on each point of the laser beam, using the following formula:

[0072] Transform all point cloud coordinates to At any given moment, motion distortion can be corrected.

[0073] Step S202: Pose calculation based on error iterative Kalman filtering. The error iterative Kalman filtering algorithm, combined with the distortion-free laser point cloud data, is used to solve the robot's pose. The specific steps are as follows: Initial pose acquisition: Initial pose is provided by wheel speed-odometer.

[0074] Residual construction: The error between the true pose and the estimated pose is defined as:

[0075] The wheel speed odometer update formula is written as:

[0076] right The linear expansion is as follows:

[0077] for right The Jacobian matrix of the derivative. For noise, obey Gaussian release , It is also a Jacobian matrix.

[0078] Start iterative solution: For the j-th iteration, construct the residual model: for each distortion-corrected map point... Convert to map coordinate system Use the kd-tree method to find the two closest points on the map. and Based on the distance from the point to the line, construct the residual:

[0079] Iterative Kalman Calculation: Residual The linearized expansion is as follows:

[0080] in, for right The Jacobian matrix of the derivative. To measure noise, conform to Gaussian distribution. .

[0081] Calculate the covariance matrix:

[0082] Calculate the Kalman gain:

[0083] in:

[0084]

[0085] Iterative pose update:

[0086] When increment The iteration ends when the value is sufficiently small or the maximum number of iterations is reached.

[0087] Update pose and covariance: , .

[0088] Step S203, QR code positioning fusion, uses the absolute position information of the QR code to correct the robot's pose. Specifically, QR codes are affixed to certain locations on the ground. When the robot passes by, the camera recognizes the QR codes to obtain the road sign information.

[0089] in, This represents the xy coordinates and yaw angle of the QR code with ID i relative to the robot.

[0090] Redefine the state as:

[0091] in, The location of the QR code with ID i on the map.

[0092] Satisfying Relationship: .

[0093] When the QR code is recognized for the first time, it can be based on The state transition equation is initialized using the robot's current pose. Similarly, combining this with the wheel speed and odometer update formula, the state transition equation is derived as follows:

[0094] and They are respectively for and noise Find the derivative of the Jacobian matrix.

[0095] The linear expansion of the measurement equation is: ; The machine pose and the pose of each QR code are updated based on Kalman filtering: Predicting covariance: ; Calculate the Kalman gain: ; Update pose and covariance: , .

[0096] Step S400: Laser point cloud map maintenance and update. The iVox voxel grid method is used to maintain and update the laser point cloud map, ensuring the sparsity and real-time performance of the map. Specific steps are as follows: Map initialization: Initialize an empty iVox voxel grid map; Point cloud insertion: Convert the laser point cloud data to the map coordinate system and insert it into the corresponding voxel grid. Only the point cloud closest to the grid center is retained for each grid. Map Updates: The map is continuously updated during the robot's movement to maintain its real-time performance and accuracy.

[0097] Step S500: Switching between positioning mode and mapping mode. The system supports both positioning and mapping modes and allows switching between them to meet the needs of different application scenarios. The specific steps are as follows: Mapping mode: Laser positioning and map update: The laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated. QR code positioning correction: When a QR code is recognized, the robot's pose is corrected based on the landmark information in the QR code.

[0098] Location mode: Loading the map: The robot loads the pre-built map and initializes its position. This requires the robot to recognize a QR code on the map. Based on the landmark information in the QR code and the map pose, the robot obtains its initial pose on the map. .

[0099] Laser positioning and map updating: Similar to the mapping mode, during machine movement, the laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated.

[0100] QR code positioning correction: Similarly, pose correction is performed when a QR code is recognized. When the machine recognizes a QR code, since the QR code's location on the map is already determined, pose correction only needs to be performed based on the QR code.

[0101] The state is defined as:

[0102] The measurement equation is:

[0103] in, The QR code's pose on the map is already a fixed value and does not need to be updated. The pose is then corrected using the same Kalman filter principle. .

[0104] The above content is merely an embodiment of the present invention. Commonly known structures and characteristics of the solutions are not described in detail here. Those skilled in the art are aware of all common technical knowledge in the field prior to the application date or priority date, are aware of all existing technologies in that field, and have the ability to apply conventional experimental methods prior to that date. Those skilled in the art can improve and implement this solution based on the guidance provided in this application and their own capabilities. Some typical known structures or methods should not be obstacles for those skilled in the art to implement this application. It should be noted that those skilled in the art can make several modifications and improvements without departing from the structure of the present invention. These should also be considered within the scope of protection of the present invention, and will not affect the effectiveness of the implementation of the present invention or the practicality of the patent. The scope of protection claimed in this application should be determined by the content of its claims, and the specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. A method for localization and mapping of an indoor robot based on lightweight multi-sensor fusion, characterized in that, include: Collect data from wheel speed encoders, laser sensors, and QR code cameras, and preprocess the collected data; The robot pose is solved by processing the laser point cloud using wheel speed odometer information, and the robot pose is solved by combining the processed laser point cloud data with the error iterative Kalman filter algorithm. The robot pose is then corrected using the position information of the QR code. The laser point cloud map is maintained and updated using the voxel grid method, which converts the laser point cloud data to the map coordinate system and inserts it into the corresponding voxel grid. The system supports mapping mode and positioning mode. In mapping mode, pose calculation and map structure update are performed simultaneously. In positioning mode, an existing map is loaded and pose calculation is performed using QR codes at known locations.

2. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 1, characterized in that, The acquisition and preprocessing of wheel speed encoder data specifically includes: Configure the encoder's resolution and counting mode; The encoder pulse count is read in each sampling period to calculate the wheel rotation distance; The linear velocity of the wheel is calculated based on the rotation distance and sampling time; Based on the dual-wheel differential drive model, the robot's pose change is estimated using the motion distance of the left and right wheels. The pose update formula is: in, For position coordinates, Yaw angle and These represent the travel distances of the left and right wheels, respectively. Given the wheelbase, the coordinate transformation matrix obtained from the pose is: .

3. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 2, characterized in that, The acquisition and preprocessing of laser sensor data specifically includes: Configure the scanning frequency and resolution of the LiDAR; The raw data from the LiDAR is read in each scanning cycle and the point cloud coordinates are obtained by parsing. The point cloud data is filtered to remove noise points and outliers.

4. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 3, characterized in that, QR code camera data acquisition and preprocessing specifically include: Configure the camera's resolution and frame rate; Read camera image data in each sampling period; The QR code recognition algorithm is used to parse the QR code information in the image to obtain the unique ID and coordinate information of the QR code; and Convert the relative coordinates of the QR code to absolute coordinates in the robot's coordinate system.

5. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 4, characterized in that, Processing laser point clouds using wheel speed and odometer information specifically refers to distortion correction, including: By utilizing the high-frequency pose information from the wheel speed odometer, the pose at any given time can be obtained through linear interpolation. For each laser point, its coordinates are transformed to the same moment based on the pose obtained by interpolation, thus achieving motion distortion correction. Previous moment up to the current moment Using high-frequency pose data from wheel speed odometers: ...through linear interpolation, we obtain Given the odometry pose at any given time, we then perform coordinate transformation on each point of the laser beam, using the following formula: Transform all point cloud coordinates to At any given moment, motion distortion can be corrected.

6. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 5, characterized in that, Solving the robot pose using the processed laser point cloud data specifically includes: Initial pose is provided by wheel speed-odometer: The error between the true pose and the estimated pose is defined as: The wheel speed odometer update formula is written as: right The linear expansion is as follows: for right The Jacobian matrix of the derivative. For noise, obey Gaussian release , It is also a Jacobian matrix.

7. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 6, characterized in that, The iterative solution process includes: For the j-th iteration, construct the residual model: for each distortion-corrected map point... Convert to map coordinate system Use the kd-tree method to find the two closest points on the map. and Based on the distance from the point to the line, construct the residual: Iterative Kalman Calculation: Residual The linearized expansion is as follows: in, for right The Jacobian matrix of the derivative. To measure noise, conform to Gaussian distribution. ; Calculate the covariance matrix: Calculate the Kalman gain: in: Iterative pose update: When increment The iteration ends when the value is sufficiently small or the maximum number of iterations is reached. Update pose and covariance: ， 。 8. The method for localization and mapping of indoor robots based on lightweight multi-sensor fusion according to claim 1, characterized in that, Using QR code location information to correct robot pose specifically includes: QR code labels with unique IDs and coordinate information are placed on the ground. When a robot passes by, it uses a camera to scan the QR code to obtain the landmark information. in, The xy coordinates and yaw angles of the QR code with ID i relative to the robot; Redefine state variables: in, The map pose of the QR code with ID i; Establish the relationship between the QR code's pose on the map and the robot's pose: ; Upon first recognition of the QR code, the robot initializes its state based on the QR code's landmark information and its current pose. The state transition equation is derived from the wheel speed odometer update formula as follows: Will and They are respectively for and noise The derivative of the Jacobian matrix linearly expands the measurement equation: ; The machine pose and the pose of each QR code are updated based on Kalman filtering: Predicting covariance: ; Calculate the Kalman gain: ; Update pose and covariance: , .

9. The method for localization and mapping of an indoor robot based on lightweight multi-sensor fusion according to claim 1, characterized in that, The mapping modes specifically include: Laser positioning and map update: The laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated. QR code positioning correction: When a QR code is recognized, the robot's pose is corrected based on the landmark information in the QR code.

10. The method for localization and mapping of an indoor robot based on lightweight multi-sensor fusion according to claim 1, characterized in that, The specific positioning modes include: Loading the map: The robot loads the pre-built map, recognizes a QR code in the map, and obtains the robot's initial pose on the map based on the road sign information in the QR code and the map pose. Laser positioning and map updating: During machine movement, the laser point cloud is distorted based on odometry, then matched with the map, and the pose is solved by iterative Kalman filtering. Then the point cloud is converted to the map and the point cloud map is updated. QR code positioning correction: When the machine recognizes the QR code, since the location of the QR code on the map is already determined, the pose is corrected based on the QR code. State variables and measurement equations are defined, and the pose is corrected based on the Kalman filter principle.

Images