ROS-based laser radar wheeled robot positioning mapping method and system

Abstract

The invention relates to the field of positioning, in particular to a laser radar wheeled robot positioning mapping method and system based on ROS. The method comprises the following steps: generating Gaussian proposal distribution by fusing laser radar and odometer data through an improved RBPF Gmapping algorithm, dynamically balancing particle degradation and computing resources in combination with an adaptive resampling threshold Neff, and reducing mapping errors and memory occupation; the optimization A * algorithm adopts heuristic function weight segmentation adjustment, redundant search directions are eliminated, and path nodes are smoothed in combination with a Bezier curve; an improved DWA algorithm introduces an obstacle trajectory prediction factor, and a speed window is dynamically adjusted; iMU and laser radar weights are dynamically distributed based on EKF multi-sensor fusion positioning, pose errors are corrected and fed back to the mapping and obstacle avoidance module, and closed-loop control is formed. According to the method, the mapping precision is improved, the path planning efficiency is improved, the positioning error is reduced, and the problems of mapping accumulative error, path redundancy and insufficient dynamic obstacle avoidance capability in a traditional scheme are effectively solved.

Description

Technical Field

[0001] The present invention relates to the field of positioning, and in particular to a ROS-based laser radar wheeled robot positioning and mapping method and system. Background Art

[0002] With the widespread adoption of tracked robots in complex and dynamic scenarios such as power inspection and logistics handling, the reliability of their positioning, mapping, and autonomous navigation technologies has become a core challenge. Existing particle filter-based RBPF Gmapping algorithms fuse odometry and lidar data to construct grid maps. However, their reliance on a fixed resampling threshold (e.g., a particle count of 30-60) leads to a loss of particle diversity and accumulation of mapping errors. These algorithms are particularly susceptible to sensor noise in dynamic environments, causing map distortion. Furthermore, traditional path planning algorithms such as A* use fixed heuristic function weights (e.g., x = 1) and an 8-way search strategy, resulting in inefficient long-range planning and numerous path breakpoints. The dynamic windowing algorithm (DWA) lacks obstacle trajectory prediction, resulting in a dynamic obstacle avoidance success rate of only 85%. Regarding multi-sensor fusion, the traditional extended Kalman filter (EKF) fails to optimize the weighting between the IMU quaternion attitude solution and lidar observations, leading to positioning errors and long-term odometry error accumulation. These issues severely restrict the navigation performance and real-time performance of robots in dynamic environments.

[0003] Patent No. CN 110007670 B discloses a method for positioning and mapping a mobile robot. However, this method's adaptability in dynamic environments needs to be improved, especially in terms of path planning and obstacle avoidance. It lacks the ability to respond quickly and avoid obstacles with a high success rate in complex dynamic scenes. Furthermore, when performing multi-sensor fusion positioning, the dynamic weight distribution of different sensor data is not optimized enough. This makes it difficult to meet the needs of tracked robots for high-precision, high-reliability positioning, mapping, and autonomous navigation in complex dynamic scenes. Summary of the Invention

[0004] The present invention provides a ROS-based lidar wheeled robot positioning and mapping method and system, aiming to solve the problems of accumulated mapping errors, low path planning efficiency, insufficient dynamic obstacle avoidance success rate, and poor multi-sensor collaborative positioning accuracy existing in traditional solutions.

[0005] To achieve the above objectives, the following technical solutions are adopted.

[0006] A lidar wheeled robot positioning and mapping method based on ROS, including:

[0007] Step S1: Construct an environmental grid map based on the improved RBPF Gmapping algorithm: Generate a Gaussian proposal distribution by fusing lidar observation data with odometry information, dynamically adjust the particle sampling range, and suppress particle divergence; determine the degree of particle degradation based on an adaptive resampling threshold Neff, and perform resampling when Neff is less than the preset threshold to generate a high-precision grid map;

[0008] Step S2: Planning a global path based on an improved A* algorithm: Based on the grid map generated in step S1, an adaptive weighting strategy using a heuristic function is employed, wherein a first weight coefficient is set for long-distance search and a second weight coefficient is set for short-distance search, and redundant search directions are eliminated to reduce the search space; path nodes are smoothed using a Bezier curve to generate a global optimal path;

[0009] Step S3: Perform local obstacle avoidance based on the improved dynamic windowing method (DWA): Based on the global path generated in step S2, a trajectory point prediction factor is introduced into the velocity sampling space to dynamically evaluate the obstacle motion trend; the velocity window is adjusted in combination with the real-time observation data of the lidar to generate an obstacle avoidance trajectory;

[0010] Step S4: Implement multi-sensor fusion positioning based on the extended Kalman filter (EKF): integrate the odometer, IMU quaternion attitude solution data, and lidar observation results, dynamically assign sensor weights, and correct the posture error; feed the corrected posture back to the grid map construction in step S1 and the obstacle avoidance trajectory generation in step S3 to form a closed-loop control.

[0011] Optionally, the process of generating a Gaussian proposal distribution in step S1 includes:

[0012] Combine lidar observation data with the odometry motion model to calculate the joint probability distribution of the particle set;

[0013] Generate a Gaussian distribution model based on the mean and covariance parameters of the sampling point set {xk};

[0014] Dynamically adjust the sampling range L(i) according to the reliability of the observed data. When the reliability is higher than the threshold, narrow the range of L(i) and reduce the number of particles.

[0015] When the reliability is lower than the threshold, Gaussian sampling is performed directly from the range of L(i) to approximate the true state distribution.

[0016] Optionally, the adaptive resampling threshold (Neff) in step S1 is calculated as follows:

[0017] The effective particle number Neff is calculated based on the normalized particle weight, and the expression is:

[0018]

[0019] where w (i) is the normalized weight of the i-th particle;

[0020] When Neff is less than the target threshold Nth, a resampling operation is performed;

[0021] When Neff is larger than Nth, resampling is skipped to preserve particle diversity.

[0022] Optionally, the method for eliminating redundant search directions in step S2 includes: in the 8-direction search strategy, dynamically disabling the search direction consistent with the obstacle extension direction according to the obstacle distribution; giving priority to retaining the search direction whose azimuth deviation from the target point is less than a preset angle, and reducing the number of search directions to less than 5.

[0023] Optionally, the Bezier curve path smoothing method in step S2 includes:

[0024] Extract key turning points in the global path;

[0025] Insert cubic Bezier curve control points between adjacent nodes to make the path curvature continuous and differentiable;

[0026] Based on the robot kinematic model constraints, the control point coordinates are adjusted to ensure trajectory feasibility.

[0027] Optionally, the method for calculating the trajectory point prediction factor in step S3 includes:

[0028] Construct obstacle motion trajectory model based on lidar historical frame data;

[0029] Predict the location distribution of obstacles in the future time window based on Kalman filtering;

[0030] The predicted position is combined with the velocity sampling space to generate an evaluation function for the obstacle avoidance trajectory.

[0031] Optionally, the sensor weight allocation method in step S4 includes: dynamically adjusting the fusion weight according to the time synchronization error between the IMU attitude matrix and the lidar observation;

[0032] When the cumulative error of the odometry exceeds the threshold, the lidar observation weight is increased to 0.7-0.9;

[0033] When lidar data is missing, switch to pure IMU-odometer fusion mode.

[0034] Optionally, the improved A* algorithm in step S2 includes the following steps:

[0035] Step S21: setting a start node and a target node according to the grid map, creating an open list and a closed list, adding the start node to the open list, and initializing the cost value of the start node;

[0036] Step S22: extract the current node with the smallest replacement value from the open list and determine whether the current node is the target node; if not, add the current node to the closed list, generate multiple adjacent nodes based on a multi-directional search strategy, dynamically eliminate redundant search directions consistent with the obstacle extension direction based on the obstacle distribution, and add the retained adjacent nodes to the open list;

[0037] Step S23, repeat step S22 until the target node is added to the closed list, extract the path node sequence from the target node in the closed list back to the start node; extract key turning nodes based on the path node sequence, insert Bezier curve control points between adjacent key turning nodes, and generate a smooth path with continuous curvature.

[0038] A ROS-based lidar wheeled robot positioning and mapping system, including:

[0039] Environmental perception module, integrating lidar, IMU and odometer, for collecting environmental data and motion status;

[0040] SLAM mapping module, which generates grid maps based on the improved RBPF Gmapping algorithm, including Gaussian proposal distribution generation unit and adaptive resampling unit;

[0041] A path planning module, comprising a global planning unit and a local obstacle avoidance unit. The global planning unit executes a heuristic function weighted A* algorithm and outputs a Bessel smoothed path. The local obstacle avoidance unit generates an obstacle avoidance trajectory based on a dynamic window method.

[0042] Multi-sensor fusion module, which uses extended Kalman filter (EKF) to fuse odometry, IMU quaternion and lidar data to dynamically correct posture errors;

[0043] The control execution module includes a motor drive unit and an encoder feedback unit, which is used to convert the planned path into motor control instructions.

[0044] Optionally, the system further includes:

[0045] Real-time communication interface, based on ROS message mechanism, connects various modules to achieve synchronous data transmission;

[0046] An exception handling unit monitors lidar data loss or IMU drift exceeding the limit, triggering weight switching or emergency braking;

[0047] The visual interactive interface displays grid maps, real-time paths, and sensor status, and supports user command input and parameter adjustment.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] This invention proposes a ROS-based positioning and mapping method and system for a tracked robot using lidar. This method uses an improved RBPF Gmapping algorithm to generate a Gaussian proposal distribution and introduces an adaptive resampling threshold, Neff, to dynamically balance particle degradation and computational resource consumption, reducing mapping error and memory usage. The A* algorithm's heuristic function weights are optimized for segmented adjustment and redundant direction elimination, reducing the number of directions from eight to five. Combined with Bezier curve path smoothing technology, this method shortens search time and path length. An improved DWA algorithm incorporates obstacle trajectory prediction factors into the evaluation function, increasing the dynamic obstacle avoidance success rate to 95%. An EKF fusion strategy with dynamic weight allocation optimizes the coordination between IMU and lidar data, reducing positioning error. This method achieves closed-loop optimization for mapping, planning, obstacle avoidance, and positioning, significantly improving navigation robustness in complex scenarios. Furthermore, the dependent claims, through refined Gaussian distribution modeling, path node smoothing, and dynamic sensor weight switching, provide multi-layered protection, ensuring comprehensive breakthroughs in real-time performance, accuracy, and stability, providing reliable technical support for the efficient operation of tracked robots in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a schematic diagram of the control flow of an embodiment of a ROS-based lidar wheeled robot positioning and mapping method of the present invention.

[0051] Figure 2 This is a flowchart of the improved RBPFGmapping algorithm for the ROS-based lidar wheeled robot positioning and mapping method of the present invention.

[0052] Figure 3 This is a flow chart of the path planning algorithm of the ROS-based lidar wheeled robot positioning and mapping method that integrates the improved A* algorithm and the improved DWA algorithm.

[0053] Figure 4 This is an overall structural diagram of an embodiment of a ROS-based lidar wheeled robot positioning and mapping system of the present invention. DETAILED DESCRIPTION

[0054] The present invention will be described in detail below with reference to the accompanying drawings and in combination with embodiments. It should be noted that, unless there is a conflict, the embodiments and features in the embodiments of the present application can be combined with each other.

[0055] The following detailed description is an exemplary description, which is intended to provide further detailed description of the present invention. Unless otherwise indicated, all technical terms used in the present invention have the same meaning as those generally understood by those skilled in the art. The terms used in the present invention are only for describing specific embodiments, and are not intended to limit the exemplary embodiments according to the present invention.

[0056] Example 1

[0057] like Figure 1 As shown, the present invention provides a method for positioning and mapping a wheeled robot using a lidar based on ROS, comprising the following steps:

[0058] S1. Constructing environmental grid maps based on the improved RBPF Gmapping algorithm

[0059] When constructing an environmental grid map, the first step is to fuse lidar observation data with odometry information to generate a Gaussian proposal distribution. Specifically, the lidar scans the environment to acquire point cloud data, forming an observation model; the odometry infers the robot's trajectory based on wheel encoder or IMU data. The lidar observation model and the odometry motion model are combined through joint probability distribution calculation to generate a joint distribution of particle sets. The state of each particle includes the robot's pose (x, y, θ), and its weight is determined by the degree of match between the observation data and the map. To optimize particle sampling efficiency, a strategy is adopted to dynamically adjust the sampling range L(i). When the reliability of the observation data is high (e.g., the lidar signal-to-noise ratio is >30dB), the range of L(i) is reduced to 60%-80% of the original value to reduce invalid particles. When the reliability is low (the signal-to-noise ratio is <20dB), Gaussian sampling is performed directly within the range of L(i), and the particle set {xk} is generated using the mean μ and covariance ∑, where μ is determined by the odometry-predicted pose and ∑ is dynamically adjusted according to the sensor noise model. For example, if the lidar noise variance is σ 2 =0.1m 2 , then the covariance matrix ∑=diag(σ 2 , σ 2 , 0.05rad 2 ).

[0060] The calculation of the adaptive resampling threshold Neff is the key to achieving a balance between particle diversity and computational efficiency. 2)Calculate the effective number of particles, where wi is the normalized weight of the i-th particle (which needs to satisfy ∑wi = 1). Set the target threshold Nth to 50% of the total number of particles (for example, when the total number of particles N = 100, Nth = 50). When Neff < Nth, perform system resampling, copy high-weight particles and eliminate low-weight particles; when Neff ≥ Nth, skip resampling to retain particle diversity. This strategy can reduce the particle dissipation problem caused by frequent resampling and reduce the memory occupancy by 30%-40% (such as from 100MB to 60MB). In actual implementation, the dynamic threshold judgment can be achieved through the particle_filter node of ROS, and the particle pose can be updated in real time in combination with the tf module.

[0061] S2. Plan the global path using the improved A* algorithm

[0062] The global path planning is implemented based on the improved A* algorithm, and its core lies in the adaptive weighting of the heuristic function and the elimination of redundant directions. First, according to the grid map generated in step S1, initialize the starting node and the target node, and create an open list (Open List) and a closed list (Close List). The heuristic function adopts a dynamic weight strategy: when searching at a long distance (such as the distance to the target > 10m), set the weight coefficient x = 2.8 to speed up the search; when searching at a short distance (the distance to the target ≤ 3m), reduce it to x = 0.85 to improve the path accuracy. To reduce redundant nodes, in the traditional 8-direction search (up, down, left, right, and diagonals), the search directions consistent with the obstacle extension direction are dynamically disabled according to the obstacle distribution. For example, if there is an obstacle on the right side of the current node, disable the right, upper right, and lower right three directions, and only retain the left, upper left, lower left, up, and down five directions. Through direction elimination, the search time can be shortened by 60% (such as from 120ms to 48ms).

[0063] The path smoothing process uses cubic Bezier curves. The specific steps include: 1) Extract the key turning points in the path node sequence (such as points where the curvature change exceeds 0.2 rad / m); 2) Insert control points between adjacent nodes. For example, insert control points P1(x1 + Δx, yy1 + Δy) and P2(x2 - Δx, y2 - Δy) between node A(X1, y1) and node B(x2, y2), where Δx and Δy are calculated according to the minimum turning radius of the robot (such as 0.5m); 3) Adjust the coordinates of the control points based on the kinematic model constraints to ensure that the path curvature is continuous and satisfies the maximum speed limit (such as 1.5m / s). In implementation, the improvement of the A* algorithm can be achieved through the global_planner package of ROS, and the bezier_curve library can be called for path smoothing. <00​​​

[0065] Local obstacle avoidance is based on the improved dynamic window method, and its core lies in the introduction of trajectory point prediction factors. First, the velocity sampling space is generated according to the global path of step S2, including the linear velocity v∈[0, v_max] and the angular velocity ω∈[-ω_max, ω_max]. The obstacle motion trajectory model is constructed using the lidar historical frame data (such as the latest 5 frames, with an interval of 0.1s per frame), and the position distribution of the obstacle in the future time window (such as 1s) is predicted by Kalman filtering. For example, for a moving obstacle, its position is predicted to be x(t+1)=x(t)+v_x·Δt, where v_x is calculated from the historical displacement. The predicted position is combined with the velocity sampling space to generate an evaluation function:

[0066] Score = α·Path Alignment + β·Obstacle Distance + γ·Speed ​​Smoothness + δ·Trajectory Prediction Deviation Score = α·Path Alignment + β·Obstacle Distance + γ·Speed ​​Smoothness + δ·Trajectory Prediction Deviation

[0067] Where α, β, Y, and δ are weight coefficients. The trajectory prediction deviation term is determined by calculating the Euclidean distance between the predicted position and the actual laser observation. If the deviation exceeds a threshold (e.g., 0.3m), the score of that velocity window is lowered. This can be implemented by extending the ROS dwa_local_planner node and integrating the kalman_filter module to achieve trajectory prediction.

[0068] S4. Multi-sensor fusion positioning (step S4)

[0069] The localization module fuses odometry, IMU quaternions, and lidar data using an extended Kalman filter (EKF). The detailed process is as follows: 1) The odometry provides displacement increments (Δx, Δy, Δθ), and the IMU uses quaternions to calculate attitude angles (roll, pitch, yaw). 2) The lidar uses ICP to match the current frame to the map to obtain pose observations. 3) Sensor weights are dynamically assigned: When the odometry cumulative error exceeds a threshold (e.g., position error > 0.5m), the lidar weight is increased to 0.8 and the IMU weight is decreased to 0.2. If lidar data is missing, the system switches to pure IMU-odometry fusion mode, with weights of 0.6 for the IMU and 0.4 for the odometry. The pose correction results are fed back to the mapping and obstacle avoidance modules via the ROS node robot_pose_ekf, forming a closed-loop control loop. For example, if the corrected pose deviates by more than 0.2m from the original estimate, a map update and path replanning are triggered.

[0070] Example 2

[0071] The system of the present invention is composed of an environmental perception module, a SLAM mapping module, a path planning module, a multi-sensor fusion module and a control execution module. The environmental perception module integrates a Velodyne VLP-16 lidar (horizontal field of view 360°, accuracy ±2cm), an Xsens MTi-30 IMU (attitude angle error <0.5°) and a wheel encoder (resolution 5000PPR), and realizes data synchronization acquisition through the rosbag tool of ROS. The SLAM mapping module is based on the improved RBPF Gmapping algorithm, and constructs a grid map (resolution 0.05m) in real time in the Gazebo simulation environment, and publishes map data through the map_server node. The global planning unit of the path planning module calls the navfn package to implement the heuristic A* algorithm, and the local obstacle avoidance unit integrates the dynamic window method through the move_base node.

[0072] The multi-sensor fusion module uses the EKF algorithm to fuse multi-source data through the robot_localization package, and dynamically adjusts the covariance matrix to optimize positioning accuracy. The control execution module includes the Maxon EC-45 motor drive unit (peak torque 0.9Nm) and the AMT102 encoder, which converts path instructions into PWM signals through the ROS ros_control framework. The real-time communication interface is based on the ROS TCP / IP protocol, and uses topics and services to synchronize data between modules. The exception handling unit triggers weight switching or emergency braking (such as speed dropping to 0) by monitoring lidar data loss (such as 3 consecutive frames not received) or IMU drift (such as attitude angle deviation >5°). The visual interactive interface is based on the RViz tool, which displays the grid map, path trajectory and sensor status in real time, and supports users to adjust parameters (such as the number of particles and path smoothness) through the GUI interface.

[0073] The robot platform uses a TurtleBot3 chassis equipped with an Intel NUC10i7 processor (16GB RAM) running the ROS Noetic system. The lidar transmits point cloud data via a USB 3.0 interface, and the IMU uploads attitude information via a serial port protocol (baud rate 115200). The motor driver board (OpenCR 1.0) receives control commands and communicates with the encoder via the CAN bus. The system is powered by a 24V lithium-ion battery pack and is equipped with an overvoltage protection circuit (threshold 30V) and a temperature sensor (monitoring range -20°C to 80°C).

[0074] Software implementation process

[0075] Initialization phase: After starting roscore, load gmapping, move_base, robot_pose_ekf and other nodes in sequence, and configure parameters through the launch file (such as the number of particles N = 100, A* weight coefficient x = 2.8 / 0.85).

[0076] Mapping and Planning: LiDAR data is pre-processed by the laser_scan_matcher node and fed into the SLAM module, which generates a grid map and publishes it to the / map topic. The global path planner subscribes to the / map and / goal topics, generates an initial path, and then calls the Bezier curve smoothing algorithm.

[0077] The obstacle avoidance and localization local planner subscribes to the odom and scan topics in real time and generates velocity commands based on the trajectory prediction results. The EKF node fuses the / odom, / imu / data, and / laser_pose topics and outputs the corrected pose to the / tf tree.

[0078] Exception handling: If the / scan topic times out (>1s), the error_handler node is triggered to switch to IMU-odometer fusion mode and send a braking command through the / emergency_stop service.

[0079] Example 3

[0080] Conceptual diagram of the ROS system involved in this invention. The ROS (Robot Operating System) system is designed to simplify the control and development of various types of robots. ROS provides a rich set of tools, libraries, and protocols, making it easier for robotics developers to build, test, and deploy various robotic applications. WillowGarage, a robotics technology company, developed ROS in 2007. Its launch had a significant impact and played a key role in improving the reusability of functional modules in robotics systems. Over time, ROS has undergone continuous iteration and improvement and has become a standard in the robotics field. ROS is described by its designers as a combination of communication mechanisms, tool packages, robotic functionality, and a robotics ecosystem. This description accurately summarizes the core features and advantages of ROS, including its flexible communication mechanisms, rich tool support, and extensive ecosystem, which make ROS an indispensable foundation in the robotics field. The emergence of ROS has greatly promoted the development and application of robotics technology, providing robotics developers with an open, flexible, and powerful development platform.

[0081] The technical route framework diagram of the tracked robot positioning, mapping and autonomous navigation involved in the present invention adopts the closed-loop research logic of "theoretical analysis-algorithm improvement-simulation verification-experimental application".

[0082] The technical route consists of four levels:

[0083] The first level is system modeling. Based on the ROS framework, a kinematic model of a Mecanum wheel robot is constructed. A multi-sensor system consisting of a lidar, an IMU, and an odometer is integrated to establish the theoretical foundation for sensor observation and data fusion.

[0084] The second level is the core algorithm improvement. A two-stage positioning strategy is proposed. The extended Kalman filter (EKF) is used to fuse multi-sensor data to achieve pose estimation. The RBPF-SLAM algorithm is improved to enhance grid map accuracy. At the same time, an improved A* algorithm and a dynamic window method are combined to build a global-local collaborative path planning system.

[0085] The third level is simulation verification, which simulates indoor scenes in the Gazebo environment and conducts quantitative comparative experiments on the optimized SLAM mapping algorithm and path planning algorithm.

[0086] The fourth level is engineering practice, which uses a physical robot platform to conduct dynamic mapping and navigation tests in real environments to verify the robustness of the algorithm in actual scenarios. A logical closed loop is formed between each level:

[0087] Theoretical models support algorithm design, simulation tests verify the feasibility of the algorithm, and ultimately the technology is implemented through engineering experiments, reflecting a research architecture of hierarchical progressiveness and modular verification.

[0088] The lidar observation model and odometry model distribution diagrams involved in this invention. Since the proposed distribution function of the RBPF Gmapping algorithm is derived from the robot's motion model and odometry data, the motion model cannot accurately reflect the actual state. The algorithm frequently resamples during operation to ensure that particles with higher weights are concentrated around the target. Therefore, particle propagation changes from sampling from the kinematic model to sampling from a Gaussian distribution. The odometry's interference resistance is far inferior to that of the lidar. Therefore, the improved algorithm proposes a distribution function that incorporates lidar and odometry data, resulting in the proposed distribution function shown in the formula.

[0089]

[0090] When the reliability of the observation data is high, the range of the observation distribution L(i) is narrow, so the motion model is used The number of new particles generated by sampling that fall within the range L(i) is relatively small. Since the number of particles used to represent the distribution of continuous space is limited, the accuracy of observation updates will be reduced. When the observation reliability is low, sampling can be performed directly from the range L(i) of the observation distribution. By using the distribution of the sampling point set {xk} to approximate the Gaussian distribution, the parameter μ of the Gaussian distribution can be calculated. (i) and Then we can use this Gaussian distribution To sample and generate new particle point sets and corresponding weights.

[0091] By considering the motion model and observation data of the sampling point set {xk}, it is approximated as a Gaussian distribution. Due to the uncertainty of the motion model, the Gaussian distribution can effectively represent the uncertainty of the state variable. The particle filter uses a group of particles to approximate the Gaussian distribution to realize the state evaluation of the robot. The Gaussian distribution is easy to calculate and process, thus simplifying the problem. Finally, the parameter μ of the Gaussian distribution is calculated using these point sets {xk}. (i) and

[0092] As shown in the following formula.

[0093]

[0094] in

[0095] The weight calculation corresponding to this proposal function becomes as follows

[0096]

[0097] Where η = 1 / p(z i |z i：t-1 ,u i：t-1 ), the improved proposed distribution function obtains the sampling samples by narrowing the sampling range and sampling the L(i) particle set This reduces the number of sampled particles and improves the accuracy of position estimation.

[0098] like Figure 2 As shown in the flowchart of the improved RBPF Gmapping algorithm involved in the present invention, resampling has a significant impact on the performance of the particle filter. Traditional resampling is prone to slow convergence and low accuracy after too many iterations. This paper adopts adaptive resampling to improve this problem. During the resampling process, low-weight particles are replaced by high-weight particles, and only a limited number of particles are used to approximate the target distribution. Therefore, it is necessary to set a resampling threshold Neff to represent the difference between the current estimate and the true value. The resampling threshold is shown in the following formula.

[0099]

[0100] in is the normalized weight of particle i. When Neff is less than the target threshold, it indicates that the particles are highly diverse and most particles deviate from the true particles, so resampling is required. When Neff is greater than the target threshold, it indicates that the differences are small and most particles are close to the true particles, so no sampling is required. This reduces the number of resampling times, thereby reducing particle dissipation and making the mapping more accurate.

[0101] like Figure 3 As shown in the flowchart of the path planning algorithm of the present invention, the improved A* algorithm and the improved DWA algorithm are integrated. The A* algorithm introduces two lists, namely the open list and the close list, to store the nodes to be searched and the nodes that have been searched. The specific algorithm flow is as follows:

[0102] (1) Initialization: Set the starting point and target point and create the variable lists open list and close list, initialize the starting point, target point, list and grid map and put the starting point into the open list.

[0103] (2) Calculate the shortest distance from the center node to the eight directions: Determine whether the target point exists at the starting point,

[0104] If it does not exist, add the starting point to the close list, add the adjacent eight nodes to the open list, and then calculate the cost of the nodes in the open list to see if there is a feasible point. If it does, end the planning. If not, continue to calculate the cost, select the node with the smallest cost, clear the nodes in the open list, and add the starting point to the close list.

[0105] (3) By repeatedly executing (2), the grid nodes that have been calculated will no longer be recalculated, and the search range will be continuously expanded until the target point appears. Finally, the shortest path is calculated and the program ends to complete the planning.

[0106] like Figure 4 As shown, the overall system structure involved in the present invention is mainly divided into three parts from top to bottom: interaction layer, algorithm layer and hardware layer. The layers cooperate with each other to jointly realize the efficient operation of the crawler robot.

[0107] Interaction layer: Establish remote communication based on Ubuntu 20.04 and ROS-galactic platform with the PC slave and Raspberry Pi to realize information interaction between users and robots, making it convenient for users to monitor the robots and issue operation instructions.

[0108] Algorithm layer: This includes the SLAM algorithm module and the path planning algorithm module. The SLAM algorithm module is responsible for environmental perception and positioning. Positioning uses the AMCL algorithm, combined with data from sensors such as lidar, to determine the robot's position within the environment. The path planning algorithm module plans the robot's motion path based on known map information, providing path decision support for the robot's autonomous navigation.

[0109] Hardware layer: This layer includes LiDAR, MPU6050 (Inertial Measurement Unit), odometer, power supply, motor, encoder, and control board. The LiDAR acquires environmental data, the MPU6050 measures the robot's posture, the odometer infers changes in the robot's position, the power supply powers each hardware device, the motor drives the robot under the control of the control board, the encoder assists in acquiring motor rotation information, and the control board coordinates the operation of each hardware device to achieve precise control of the robot's motion.

[0110] Through such a layered structural design, the division of labor between each layer is clear and closely coordinated, enabling the tracked robot to realize functions such as autonomous navigation, environmental perception and interaction in complex environments, thereby improving the robot's intelligence level and operating efficiency.

[0111] It is understood from common technical knowledge that the present invention may be implemented by other embodiments that do not depart from its spirit or essential features. Therefore, the embodiments disclosed above are, in all respects, merely illustrative and not exclusive. All modifications within the scope of the present invention or equivalent to the scope of the present invention are intended to be encompassed by the present invention.

Claims

1. A laser radar wheeled robot positioning and mapping method based on ROS, characterized in that: include, Step S1: Construct an environmental grid map based on the improved RBPF Gmapping algorithm: Generate a Gaussian proposal distribution by fusing lidar observation data with odometry information, dynamically adjust the particle sampling range, and suppress particle divergence; determine the degree of particle degradation based on an adaptive resampling threshold Neff, and perform resampling when Neff is less than the preset threshold to generate a high-precision grid map; Step S2: Planning a global path based on an improved A* algorithm: Based on the grid map generated in step S1, an adaptive weighting strategy using a heuristic function is employed, wherein a first weight coefficient is set for long-distance search and a second weight coefficient is set for short-distance search, and redundant search directions are eliminated to reduce the search space; path nodes are smoothed using a Bezier curve to generate a global optimal path; Step S3: Perform local obstacle avoidance based on the improved dynamic windowing method (DWA): Based on the global path generated in step S2, a trajectory point prediction factor is introduced into the velocity sampling space to dynamically evaluate the obstacle motion trend; the velocity window is adjusted in combination with the real-time observation data of the lidar to generate an obstacle avoidance trajectory; Step S4: Implement multi-sensor fusion positioning based on the extended Kalman filter (EKF): integrate the odometer, IMU quaternion attitude solution data, and lidar observation results, dynamically assign sensor weights, and correct the posture error; feed the corrected posture back to the grid map construction in step S1 and the obstacle avoidance trajectory generation in step S3 to form a closed-loop control.

2. A ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The process of generating the Gaussian proposal distribution in step S1 includes: Combine lidar observation data with the odometry motion model to calculate the joint probability distribution of the particle set; Generate a Gaussian distribution model based on the mean and covariance parameters of the sampling point set {xk}; Dynamically adjust the sampling range L(i) according to the reliability of the observed data. When the reliability is higher than the threshold, narrow the range of L(i) and reduce the number of particles. When the reliability is lower than the threshold, Gaussian sampling is performed directly from the range of L(i) to approximate the true state distribution.

3. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1 is characterized in that: The adaptive resampling threshold Neff in step S1 is calculated as follows: The effective particle number Neff is calculated based on the normalized particle weight, and the expression is: where w (i) is the normalized weight of the i-th particle; When Neff is less than the target threshold Nth, a resampling operation is performed; When Neff is larger than Nth, resampling is skipped to preserve particle diversity.

4. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The method for eliminating redundant search directions in step S2 includes: in the 8-direction search strategy, dynamically disabling the search direction consistent with the obstacle extension direction according to the obstacle distribution; and preferentially retaining the search direction whose azimuth deviation from the target point is less than a preset angle, thereby reducing the number of search directions to less than 5.

5. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The Bezier curve path smoothing method in step S2 includes: Extract key turning points in the global path; Insert cubic Bezier curve control points between adjacent nodes to make the path curvature continuous and differentiable; Based on the robot kinematic model constraints, the control point coordinates are adjusted to ensure trajectory feasibility.

6. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The method for calculating the trajectory point prediction factor in step S3 includes: Construct obstacle motion trajectory model based on lidar historical frame data; Predict the location distribution of obstacles in the future time window based on Kalman filtering; The predicted position is combined with the velocity sampling space to generate an evaluation function for the obstacle avoidance trajectory.

7. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The sensor weight allocation method in step S4 includes: dynamically adjusting the fusion weight according to the time synchronization error between the IMU attitude matrix and the lidar observation; When the cumulative error of the odometry exceeds the threshold, the lidar observation weight is increased to 0.7-0.9; When lidar data is missing, switch to pure IMU-odometer fusion mode.

8. The ROS-based laser radar wheeled robot positioning and mapping method according to claim 1, characterized in that: The improved A* algorithm in step S2 includes the following steps: Step S21: setting a start node and a target node according to the grid map, creating an open list and a closed list, adding the start node to the open list, and initializing the cost value of the start node; Step S22: extract the current node with the smallest replacement value from the open list and determine whether the current node is the target node; if not, add the current node to the closed list, generate multiple adjacent nodes based on a multi-directional search strategy, dynamically eliminate redundant search directions consistent with the obstacle extension direction based on the obstacle distribution, and add the retained adjacent nodes to the open list; Step S23, repeat step S22 until the target node is added to the closed list, extract the path node sequence from the target node in the closed list back to the start node; extract key turning nodes based on the path node sequence, insert Bezier curve control points between adjacent key turning nodes, and generate a smooth path with continuous curvature.

9. A ROS-based laser radar wheeled robot positioning and mapping system, based on a ROS-based laser radar wheeled robot positioning and mapping method according to any one of claims 1 to 8, characterized in that: include: Environmental perception module, integrating lidar, IMU and odometer, for collecting environmental data and motion status; SLAM mapping module, which generates grid maps based on the improved RBPF Gmapping algorithm, including Gaussian proposal distribution generation unit and adaptive resampling unit; A path planning module, comprising a global planning unit and a local obstacle avoidance unit. The global planning unit executes a heuristic function weighted A* algorithm and outputs a Bessel smoothed path. The local obstacle avoidance unit generates an obstacle avoidance trajectory based on a dynamic window method. Multi-sensor fusion module, which dynamically corrects posture errors by fusing odometer, IMU quaternion and lidar data through extended Kalman filter (EKF); The control execution module includes a motor drive unit and an encoder feedback unit, which is used to convert the planned path into motor control instructions.

10. The ROS-based laser radar wheeled robot positioning and mapping system according to claim 9, characterized in that: The system further comprises: Real-time communication interface, based on ROS message mechanism, connects various modules to achieve synchronous data transmission; An exception handling unit monitors lidar data loss or IMU drift exceeding the limit, triggering weight switching or emergency braking; The visual interactive interface displays grid maps, real-time paths, and sensor status, and supports user command input and parameter adjustment.

Images