A service robot path planning method suitable for indoor environment

Abstract

The application relates to the technical field of robots, and provides a service robot path planning method suitable for an indoor environment, which comprises the following steps: acquiring point cloud data of a laser radar, inertial data of an inertial measurement unit and displacement data of an odometer, and organically integrating high-precision time synchronization, real-time mapping positioning, global path guidance and local obstacle avoidance decision together. In this way, the accuracy of environment perception is effectively improved, a reasonable driving path is planned based on a dynamic map and the position of the robot, the traffic efficiency is guaranteed, and collision with obstacles in the scene is avoided, so that the obstacle avoidance behavior has safety and global directionality, and the navigation reliability, smoothness and intelligence level of the robot in a complex dynamic indoor environment are enhanced.

Description

Technical Field

[0001] This application relates to the field of robotics technology, and in particular to a path planning method for service robots suitable for indoor environments. Background Technology

[0002] With the rapid development of applications such as smart homes, unmanned delivery, and smart buildings, indoor service robots are becoming increasingly popular. Their working environments are typically complex, unstructured scenarios containing elements such as dense furniture, moving pedestrians, and narrow passages. In such environments, the core prerequisite for robots to achieve safe, efficient, and autonomous navigation lies in relying on accurate and reasonable path planning.

[0003] This requires the robot to have environmental distance perception and real-time, reliable dynamic obstacle avoidance capabilities. That is, by building a dynamic map consistent with the current environment in real time, and planning a reasonable driving path based on the dynamic map and the robot's position, the robot can ensure traffic efficiency and avoid collisions with obstacles in the scene. Summary of the Invention

[0004] This application provides a service robot path planning method suitable for indoor environments, which plans a reasonable driving path for the service robot in an indoor environment, ensuring both passage efficiency and avoiding collisions with obstacles in the scene.

[0005] This application provides a service robot path planning method suitable for indoor environments, including:

[0006] The system acquires point cloud data from the lidar, inertial data from the inertial measurement unit, and displacement data from the odometer, and synchronizes the point cloud data, inertial data, and displacement data in time. The lidar, inertial measurement unit, and odometer are mounted on the robot.

[0007] The point cloud data is scanned and matched with the current local sub-graph to determine the robot's current pose, and the local sub-graph is updated based on the motion state filtering results.

[0008] Obtain the robot's starting point and target point, and determine the global path of the robot from the starting point to the target point based on the global map;

[0009] Based on the robot's current pose, the look-ahead point is extracted on the global path, and the expected yaw angle of the look-ahead point is determined. The expected yaw angle is the target motion direction set for the robot based on the global path.

[0010] Based on the time-synchronized point cloud data, inertial data, displacement data, local subgraph, global path, and expected yaw angle, the final execution trajectory is determined. The final execution trajectory includes the linear velocity and angular velocity used to drive the robot's motion.

[0011] In one implementation method, the step of determining the final execution trajectory based on time-synchronized point cloud data, inertial data, displacement data, local sub-map, global path, and desired yaw angle includes:

[0012] Based on time-synchronized point cloud data, inertial data, and displacement data, as well as local sub-maps, global paths, and desired yaw angles, multiple predicted trajectories are sampled and simulated in the velocity space.

[0013] An evaluation function that includes the deviation between the trajectory endpoint yaw angle and the expected yaw angle is used to score multiple predicted trajectories, and the trajectory with the highest score is selected as the final execution trajectory.

[0014] In one implementation, before selecting the trajectory with the highest score as the final execution trajectory, the following steps are also included:

[0015] Construct a cost map for evaluating the predicted trajectory and set the dilation layer parameters of the cost map, where the dilation radius is... satisfy: ; The physical radius of the robot. This is the inertial gliding distance required for the robot to brake at the current speed. This is an additional safety margin.

[0016] If the predicted trajectory intersects with a high-cost region in the cost map, the predicted trajectory is discarded.

[0017] In one implementation method, the step of time synchronization of point cloud data, inertial data, and displacement data also includes:

[0018] Analyze the timestamps of point cloud data, inertial data, and displacement data;

[0019] A reference time axis is established using the continuous high-frequency data stream from the inertial measurement unit, and the dynamic delay of point cloud data and displacement data relative to the reference time axis is determined.

[0020] Based on dynamic delay, cubic spline interpolation is used to align point cloud data and displacement data to the reference time axis to generate time-synchronized point cloud data, inertial data, and displacement data.

[0021] In one implementation, before the step of scanning and matching the point cloud data with the current local sub-graph to determine the robot's current pose, the following steps are also included:

[0022] The input point cloud data is downsampled and filtered using the initial maximum voxel size;

[0023] Determine whether the point cloud density of the downsampled point cloud data is lower than the preset minimum point count threshold;

[0024] If the point cloud density of the downsampled point cloud data is lower than the preset minimum point number threshold, the voxel size is reduced and downsampling filtering is performed again until the downsampled point cloud density is greater than or equal to the minimum point number threshold.

[0025] In one implementation, the step of scanning and matching point cloud data with the current local sub-image to determine the robot's current pose also includes:

[0026] A global search is performed in a coarse-resolution likelihood field to obtain multiple candidate poses for the robot.

[0027] Local optimization is performed on multiple candidate poses in a fine-resolution likelihood field to determine the optimal estimated pose among the multiple candidate poses as the current pose.

[0028] In one implementation method, the step of updating the local subgraph based on the motion state filtering result includes:

[0029] Calculate the deviation between the angular velocity in the inertial data and the angular velocity in the displacement data;

[0030] Based on the comparison results between the deviation and the preset threshold, the robot's motion state is selectively judged according to displacement or inertia data.

[0031] If the motion state is determined to be a reliable motion state, the current local sub-graph is updated based on the point cloud data of the current frame; if the motion state is determined to be a stationary state or a low-speed motion state, the point cloud data of the current frame is discarded. A reliable motion state means that the robot's linear velocity and angular velocity both meet the preset conditions.

[0032] In one implementation, the step of updating the current local sub-graph based on the point cloud data of the current frame also includes:

[0033] Continuously count the number of frames of point cloud data used to update the current local subgraph;

[0034] When the number of frames reaches a preset frame threshold, the current local subgraph is frozen and stored in the historical subgraph queue, and a new local subgraph is created to receive subsequent point cloud data.

[0035] One implementation method also includes:

[0036] The point cloud data of the current frame is matched with the subgraphs in the historical subgraph queue to perform loop closure detection;

[0037] When a closed loop is detected, a pose graph containing the robot's historical pose nodes and constraint edges is constructed.

[0038] The pose graph is optimized to correct the robot's historical pose and global map.

[0039] In one implementation, the step of extracting look-ahead points on the global path based on the robot's current pose includes:

[0040] Starting from the matching point of the robot's current pose on the global path, accumulate the Euclidean distance between subsequent path points and generate the cumulative length;

[0041] When the cumulative length reaches the preset look-ahead distance, the corresponding waypoint is determined as the look-ahead point, and the expected yaw angle is the azimuth angle of the line connecting the look-ahead point and the next waypoint.

[0042] This application provides a service robot path planning method suitable for indoor environments, including: acquiring point cloud data from a LiDAR, inertial data from an inertial measurement unit (IMU), and displacement data from an odometry system (OPS), and synchronizing the point cloud data, inertial data, and displacement data in time. The LiDAR, IMU, and OPS are mounted on the robot. The point cloud data is scanned and matched with a current local sub-map to determine the robot's current pose, and the local sub-map is updated based on motion state filtering results. The robot's starting point and target point are acquired, and a global path from the starting point to the target point is determined based on a global map. A look-ahead point is extracted from the global path based on the robot's current pose, and the expected yaw angle of the look-ahead point is determined. The expected yaw angle is the target motion direction set for the robot based on the global path. The final execution trajectory is determined based on the time-synchronized point cloud data, inertial data, displacement data, local sub-map, global path, and expected yaw angle. The final execution trajectory includes the linear velocity and angular velocity used to drive the robot's motion.

[0043] In this way, by organically integrating high-precision time synchronization, real-time mapping and localization, global path guidance, and local obstacle avoidance decision-making, the accuracy of environmental perception is effectively improved. Based on the dynamic map and the robot's position, a reasonable driving path is planned, ensuring both traffic efficiency and avoiding collisions with obstacles in the scene. This makes obstacle avoidance behavior both safe and globally directional, enhancing the robot's navigation reliability, smoothness, and intelligence in complex dynamic indoor environments. Attached Figure Description

[0044] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0045] Figure 1 A flowchart illustrating a service robot path planning method suitable for indoor environments, provided in this application embodiment. Figure 1 ;

[0046] Figure 2 A flowchart illustrating a service robot path planning method suitable for indoor environments, provided in this application embodiment. Figure 2 ;

[0047] Figure 3 A flowchart illustrating a service robot path planning method suitable for indoor environments, provided in this application embodiment. Figure 3 ;

[0048] Figure 4 A schematic diagram of a service robot path planning method suitable for indoor environments provided in this application embodiment. Figure 4 .

[0049] Figure 5 A schematic diagram of a service robot path planning method suitable for indoor environments provided in this application embodiment. Figure 5 .

[0050] Figure 6 This is a schematic diagram of a service robot path planning device suitable for indoor environments, provided as an embodiment of this application. Detailed Implementation

[0051] The embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described below do not represent all embodiments consistent with this application. They are merely examples of systems and methods consistent with some aspects of this application as detailed in the claims.

[0052] It should be noted that the brief descriptions of terms in this application are only for the convenience of understanding the embodiments described below, and are not intended to limit the embodiments of this application. Unless otherwise stated, these terms should be understood in their ordinary and common meaning.

[0053] In this application, the terms "first," "second," "third," etc., used in the specification, claims, and accompanying drawings are used to distinguish similar or related objects or entities, and do not necessarily imply a specific order or sequence, unless otherwise specified. It should be understood that such terms are interchangeable where appropriate.

[0054] The terms include and have, and any variations thereof, are intended to cover but not exclusively include, for example, a product or device that includes a range of components is not necessarily limited to all of the components that are clearly listed, but may include other components that are not clearly listed or that are inherent to such product or device.

[0055] The term module refers to any known or subsequently developed hardware, software, firmware, artificial intelligence, fuzzy logic, or combination of hardware and / or software code that is capable of performing the functions associated with that element.

[0056] Indoor service robots (such as cleaning robots and delivery robots) perform tasks in complex environments such as homes and offices. These environments contain dense furniture, moving people, and narrow passageways. When navigating autonomously in such dynamic and complex environments, robots need to plan reasonable paths to ensure efficient passage while avoiding collisions with obstacles in the scene.

[0057] To address the aforementioned technical problems, embodiments of this application provide a service robot path planning method suitable for indoor environments, such as... Figure 1 As shown, it includes steps S110 to S150.

[0058] Step S110: Acquire point cloud data from the lidar, inertial data from the inertial measurement unit, and displacement data from the odometer, and synchronize the point cloud data, inertial data, and displacement data in time.

[0059] The LiDAR, inertial measurement unit, and odometer are mounted on the robot. The robot uses its own LiDAR, inertial measurement unit, and odometer to collect point cloud data, inertial data, and displacement data of the environment.

[0060] LiDAR (Light Detection and Ranging) is a sensor that acquires three-dimensional information about the environment through laser ranging and dynamic scanning. It emits a near-infrared laser beam at a fixed frequency, which is guided by a scanning mechanism to cover the surrounding space at a specific angle. When the laser beam encounters an obstacle, part of the light is reflected, and the reflected light is captured by a photoelectric receiver. Then, by stitching and denoising the continuously acquired point cloud in real time, a three-dimensional contour model of the surrounding environment can be constructed, clearly showing the position, shape, distance, and other information of obstacles. This provides accurate environmental perception data for scenarios such as robot navigation and autonomous driving.

[0061] An inertial measurement unit (IMU) is a sensor that uses internal sensitive elements to sense changes in motion and attitude to obtain the real-time motion status of a device. It has two core components: a gyroscope and an accelerometer. It collects data at a fixed frequency and performs calibration and attitude calculation, and can output the device's attitude angle, motion trajectory and velocity information in real time.

[0062] The odometer monitors the displacement changes of the moving parts of the device, and the raw displacement data collected needs to be calibrated for error before finally outputting the instantaneous speed, cumulative mileage, and coordinates relative to the starting point in real time.

[0063] After acquiring point cloud data, inertial data, and displacement data, a high-precision time synchronization algorithm is executed to unify data from different sources with different timestamps onto the same time base, providing a consistent data foundation for subsequent processing.

[0064] Step S120: Scan and match the point cloud data with the current local sub-graph to determine the robot's current pose, and update the local sub-graph based on the motion state filtering results.

[0065] To achieve local map construction and robot localization, time-synchronized laser point clouds are used. A scanning matching algorithm is employed to match the current point cloud with the constructed local sub-map, so as to accurately calculate the robot's position and orientation in the current sub-map, i.e., the robot's current pose.

[0066] Specifically, the Correlative Scan Matching (CSM) algorithm is used for scan matching. The CSM algorithm solves the scan matching problem by constructing coarse-resolution and fine-resolution likelihood field models. First, a global search is performed in the coarse-resolution likelihood field. This is done by dividing the search space into low-resolution grids, calculating the matching error of the pose corresponding to each grid vertex, and retaining the top N candidate poses with the smallest errors, thus obtaining multiple candidate poses for the robot. Then, a finer search is performed in the fine-resolution likelihood field on the grids related to the optimal pose. Local optimization is performed on the multiple candidate poses to obtain the optimal pose at a higher resolution, i.e., the optimal estimated pose among the multiple candidate poses is determined as the current pose. Through this step-by-step search process, the CSM algorithm can accurately estimate the robot's position at different resolutions, achieving efficient scan matching.

[0067] In this embodiment, when the robot performs scanning matching, it needs to quickly and accurately find the best matching pose between the current point cloud and the map within a large range of positional and angular uncertainties. To resolve the contradiction between the high computational cost of global search and the tendency of local search to get trapped in local optima during scanning matching, a global search is first performed in a coarse-resolution likelihood field. This search covers a large area, encompassing all possible locations where the robot might exist, and yields multiple candidate poses. Then, within the neighborhood of the several best candidate poses obtained in the previous step, a more refined search is performed using a high-resolution likelihood field map. Through iterative optimization, the optimal estimated pose is finally determined from the candidate poses. Thus, by employing a two-stage search strategy from coarse to fine, combining the robustness of global search with the accuracy of local search, it avoids getting trapped in local optima while ensuring computational efficiency, achieving real-time and accurate localization.

[0068] In constructing the environmental map, the decision to update the local submap using current data is based on the robot's real-time motion state (e.g., whether it is stationary). Specifically, for example... Figure 2 As shown, it includes steps S121 to S123.

[0069] Step S121: Calculate the deviation between the angular velocity in the inertial data and the angular velocity in the displacement data.

[0070] When a robot travels on a smooth surface, its wheels may slip. In such cases, relying solely on the displacement data from the odometer to determine the robot's motion state may be inaccurate. Therefore, this embodiment calculates the deviation between the angular velocity provided by the inertial measurement unit and the angular velocity provided by the odometer in real time.

[0071] Step S122: Based on the comparison result between the deviation and the preset threshold, the robot motion state is selectively judged according to the displacement or inertial data.

[0072] If the deviation between the angular velocity provided by the inertial measurement unit and the angular velocity provided by the odometer is less than a preset first threshold, the motion state is determined based on the linear velocity and angular velocity provided by the displacement data of the odometer.

[0073] If the deviation is higher than or equal to the first threshold, the wheeled structure of the odometer is considered to be slipping, and the measurement data is unreliable. The robot's motion state is then determined based on the angular velocity provided by the inertial measurement unit's inertial data.

[0074] Step S123: If the motion state is determined to be a reliable motion state, then update the current local sub-graph based on the point cloud data of the current frame; if the motion state is determined to be a stationary state or a low-speed motion state, then discard the point cloud data of the current frame.

[0075] A reliable motion state refers to a robot whose linear velocity and angular velocity both meet preset conditions. For example, judging the motion state based on the linear velocity and angular velocity provided by the odometry displacement data means that if the absolute values ​​of both the linear velocity and angular velocity provided by the odometry are higher than the corresponding preset thresholds, the robot is determined to be in a reliable motion state. If the absolute values ​​of both the linear velocity and angular velocity are lower than the corresponding preset thresholds, the robot is determined to be stationary or in a low-speed state.

[0076] For example, determining motion state based on inertial data means that if the absolute value of the angular velocity provided by the inertial data is higher than or equal to a corresponding preset threshold, the robot is determined to be in motion; if the absolute value of the angular velocity provided by the inertial data is lower than the corresponding preset threshold, the robot is determined to be stationary or in a low-speed state. The specific value of the preset threshold is set by technicians based on experience, or according to the specific scenario. For example, the preset threshold will differ depending on whether the scenario only contains equipment or contains both equipment and personnel.

[0077] This application embodiment uses motion filtering and subgraph management to filter reliable data and dynamically update the current local map, avoiding noise contamination of the map while ensuring local consistency of the subgraph. When the robot is stationary or moving at low speed, the pose estimation error of the scanning matching will increase significantly (the proportion of sensor noise increases). At this time, a speed threshold is used to discard the current laser scan frame to avoid inserting noisy points into the map and ensure the cleanliness of the subgraph. The subgraph is the basic unit of the local map. Initially, a baseline subgraph is created, and motion-filtered laser point clouds are continuously inserted. The grid probability is updated iteratively using Bayes' theorem. Each time a laser point cloud is added, the occupancy probability of the corresponding grid is updated according to whether the point falls on an obstacle. This process is repeated to construct environmental features.

[0078] When the number of scan frames accumulated in a submap reaches a threshold, no further updates or new baseline submaps are created. These submaps serve as historical references for subsequent matching with new submaps (supporting loop closure detection). Submaps at different time steps reflect the current environment in real time, ensuring the timeliness of the local map. If the pose data is reliable (not discarded), it is used to update the local submap online, in the form of voxel grids (each grid indicating whether there is an obstacle passage cost), thus building or updating the local environment map around the robot in real time.

[0079] In this way, the robustness of motion state judgment is improved through cross-validation between the inertial measurement unit and the odometer. It can effectively identify and handle odometer failures, ensuring that the mapping process remains stable even under non-ideal conditions and avoiding the contamination of the map by erroneous data.

[0080] Step S130: Obtain the robot's starting point and target point, and determine the global path of the robot from the starting point to the target point based on the global map.

[0081] On a known global map, a globally optimal path from the starting point to the target point is planned based on the target point given by the user.

[0082] Step S140: Extract the look-ahead point on the global path based on the robot's current pose, and determine the expected yaw angle of the look-ahead point. The expected yaw angle is the target motion direction set for the robot based on the global path.

[0083] Based on the robot's current pose, a look-ahead point at a certain distance ahead is selected on the global path, and the direction of this point (expected yaw angle) is calculated as the robot's motion direction guide. The expected yaw angle is the tangent direction of the look-ahead point on the global path. Specifically, the expected yaw angle is determined based on the direction of the line connecting the look-ahead point to one or more subsequent path points on the global path.

[0084] Step S150: Based on the time-synchronized point cloud data, inertial data, displacement data, local sub-map, global path, and expected yaw angle, determine the final execution trajectory.

[0085] The final execution trajectory includes the linear velocity and angular velocity used to drive the robot's motion.

[0086] Thus, this embodiment of the application organically integrates high-precision time synchronization, real-time mapping and positioning, global path guidance, and local obstacle avoidance decision-making. This effectively improves the accuracy of environmental perception, and based on the dynamic map and the robot's position, plans a reasonable driving path that ensures both traffic efficiency and avoids collisions with obstacles in the scene. This makes obstacle avoidance behavior both safe and globally directional, enhancing the robot's navigation reliability, smoothness, and intelligence in complex dynamic indoor environments.

[0087] During robot movement, the robot moves along a global path. Dynamic obstacles (such as pedestrians or vehicles) may suddenly appear along this path. In such cases, a new local trajectory needs to be planned immediately to control the robot to avoid them. For example... Figure 3 As shown, the embodiments of this application include steps S310 to S320.

[0088] Step S310: Based on the time-synchronized point cloud data, inertial data, and displacement data, as well as the local sub-map, global path, and expected yaw angle, sample and simulate multiple predicted trajectories in the velocity space.

[0089] Specifically, based on the robot's current speed and acceleration, future linear and angular velocities are sampled in combination within a dynamic window. For each set of speed and acceleration, the robot's kinematic model is used to simulate and predict its trajectory over the next few seconds, thereby generating multiple candidate predicted trajectories.

[0090] Step S320: Use an evaluation function that includes the deviation between the trajectory endpoint yaw angle and the expected yaw angle to score multiple predicted trajectories, and select the trajectory with the highest score as the final execution trajectory.

[0091] Specifically, for each candidate trajectory, an evaluation function is used to score it, and the trajectory with the highest score is selected as the final execution trajectory. The core evaluation metric of this function is the deviation between the robot's orientation (yaw angle) at the trajectory's endpoint and the desired yaw angle. The smaller the deviation, the higher the score. This ensures that the robot will choose trajectories that align its body direction with the global path's forward direction.

[0092] The expression for the evaluation function J is: .

[0093] in, and To predict the lateral and longitudinal positional deviations between the trajectory endpoint and the look-ahead point, To predict the deviation between the yaw angle at the end of the trajectory and the expected yaw angle, , , These are the corresponding weighting coefficients; where, the weighting coefficients Much larger than the weighting coefficient and This allows the evaluation function to prioritize predicted trajectories with smaller yaw angle deviations. By directly integrating global path direction guidance (through the expected yaw angle) into local obstacle avoidance decisions, the robot can effectively avoid deviating from its global direction or even getting stuck due to local obstacle avoidance, significantly improving the intelligence and success rate of navigation.

[0094] In practical applications, robots need to pass near obstacles (such as table legs or walls) while ensuring that they do not collide with the obstacles and maintain a reasonable safe distance from them.

[0095] To ensure the safety of the final execution trajectory, before selecting the trajectory with the highest score as the final execution trajectory, a cost map is constructed to evaluate the predicted trajectory, and the inflation layer parameters of the cost map are set.

[0096] Specifically, firstly, a cost map is generated based on the real-time local submap. Each grid cell in the map is assigned a cost value, with the area containing obstacles having the highest cost value. Then, the obstacles are inflated. In practical applications, the inflated radius... It's not fixed, but calculated dynamically.

[0097] Cost map expansion radius It needs to simultaneously cover the robot's physical dimensions, inertial braking distance, operational safety margin, and its expansion radius. satisfy: .in, The physical radius of the robot. This is the inertial gliding distance required for the robot to brake at the current speed. To provide an additional safety margin, the expanded high-risk area is designed to cover all potential collision risks. This ensures the expanded area completely covers the robot's outline and braking risk zone.

[0098] To ensure that the cost within the expansion layer gradually decreases with increasing distance from the obstacle, and that only the central region is a low-cost zone.

[0099] .

[0100] if This indicates that sticking close to an obstacle is set to a high cost; if it is... Indicates a gradual decline, if This indicates no risk and sets the cost to the minimum.

[0101] in, R_min is the distance, referring to the Euclidean distance from the current grid cell (a small cell in the map) to the nearest obstacle. R_min is the inscribed radius (distance threshold), when... When the grid is inside an obstacle or has intruded into the robot's safe distance, it indicates that the grid is located inside an obstacle or has intruded into the robot's safe distance. The maximum cost value refers to the upper limit of the cost value, when When the function returns directly... . The attenuation coefficient is a parameter used to control the rate at which the cost value decays. The larger the value, the greater the cost with distance. The more the cost increases, the slower the decay occurs; that is, the larger the high-cost region and the smoother the transition. Conversely, The smaller the value, the faster the decay, and the steeper the transition from the safe zone to the danger zone. Minimum cost value refers to the lower limit of the cost value, when As the exponent approaches infinity, the exponent term approaches 0, therefore It tends towards 0. However, in practical applications, a value is set. (Usually 0) as the cost of free space.

[0102] Before trajectory evaluation, all candidate trajectories are compared with the cost map. If the predicted trajectory intersects with a high-cost region in the cost map, the predicted trajectory is removed. That is, if any point on a trajectory falls within an inflated region where the cost value exceeds the safety threshold, the trajectory is determined to be a dangerous trajectory and is directly removed from the candidate list.

[0103] Thus, this application establishes an efficient safety screening mechanism by introducing a cost map that considers robot dynamics and safety margins. This mechanism can eliminate all unsafe options early in trajectory planning, ensuring that subsequent scoring processes only involve the set of safe trajectories, thereby fundamentally avoiding collision risks and improving system safety. The hardware components and software modules in this application are mounted on the robot to achieve navigation functionality.

[0104] In practical applications, the acquisition frequencies of lidar, inertial measurement unit, and odometer may be different. For example, the acquisition frequency of lidar is 10Hz, that of inertial measurement unit is 100Hz, and that of odometer is 20Hz. If the acquired data is used directly, it will cause ghosting or positioning drift in environmental perception.

[0105] In the steps of time synchronization of point cloud data, inertial data, and displacement data, such as... Figure 4 As shown, it also includes steps S410 to S430.

[0106] Step S410: parse the timestamps of point cloud data, inertial data, and displacement data.

[0107] Step S420: Establish a reference time axis using the continuous high-frequency data stream of the inertial measurement unit, and determine the dynamic delay of the point cloud data and displacement data relative to the reference time axis.

[0108] Based on the current system time, a reasonable time deviation threshold is set (determined by the sensor frame rate and communication latency), and an effective time interval is defined:

[0109] .

[0110] in The current system time. To determine the maximum permissible time deviation threshold, the timestamp of each frame of sensor data (point cloud data, inertial data, displacement data, and odometer data) is verified. If the data does not belong to the specified range, it is marked and removed as abnormal data.

[0111] A time synchronization coordinate system is established using the continuous high-frequency data stream from the inertial measurement unit as the reference time axis. The delay deviation of the lidar point cloud data timestamp relative to the reference axis is calculated. Specifically, the steps for calculating the dynamic delay include: calculating in real time the difference between the timestamp of each frame of data from the lidar and odometer and the corresponding moment on the reference time axis to obtain the instantaneous delay. To further smooth out the fluctuations, a sliding window (such as the most recent 20 frames) is used to calculate the average of these delays. This serves as the current dynamic compensation amount.

[0112] For example, setting a reasonable time deviation threshold (determined by sensor frame rate and communication latency) and defining the valid time interval for timestamps:

[0113] .

[0114] in This is the current system time (based on the inertial measurement unit). To determine the maximum allowable time deviation threshold, the timestamp of each frame of sensor data is verified. If the data does not belong to this range, it is marked and the abnormal data is removed.

[0115] The dynamic delay is determined based on the timestamp of the inertial measurement unit. Specifically, the delay model for a certain sensor (such as a lidar) in the k-th frame is as follows:

[0116] .

[0117] The dynamic compensation is calculated using the most recent N frames (the window size N is determined by the sensor frame rate). For each new frame of data, the window slides once and is updated. as follows:

[0118] .

[0119] in This is the timestamp of the k-th frame of data from the inertial measurement unit. This is the timestamp of the k-th frame of data from a sensor (such as a lidar). The sensor delay relative to the inertial measurement unit in the k-th frame, To adjust the sliding window size, This represents the dynamic average delay (compensation amount) of the k-th frame.

[0120] This involves maintaining a fixed-size sliding time window containing recent multi-frame point cloud data along with corresponding inertial and displacement data. Within this window, the deviation between the timestamp of each frame of point cloud data and the corresponding moment on the reference axis is calculated. All deviations within the sliding time window are statistically analyzed, and the statistical results are used as the current dynamic delay deviation estimate. Then, based on the delay deviation, the timestamps of the point cloud and displacement data are dynamically compensated to align them with the reference axis.

[0121] Step S430: Based on dynamic delay, cubic spline interpolation is used to align point cloud data and displacement data to the reference time axis to generate time-synchronized point cloud data, inertial data and displacement data.

[0122] Dynamic timestamp compensation is performed. Using the timestamp of the inertial measurement unit as the reference time axis, the dynamic delay mean of the lidar and odometer is calculated through a sliding window. Spline interpolation is performed on the continuous data. Cubic spline interpolation can ensure the smoothness of the interpolation curve through the continuity of the second derivative of adjacent data points, avoiding jumps in the lidar point cloud data due to interpolation and ensuring the continuity of the surrounding environment. Then, data that perfectly matches the target timestamp is generated, and the compensation amount is updated in real time, thereby realizing the correct mapping of the lidar and odometer timestamps onto this axis.

[0123] Data time alignment uses cubic spline interpolation to interpolate discrete sensor data to each target time point on the reference time axis of the inertial measurement unit, ensuring the continuity of the second derivative of the interpolation curve (smooth and without jumps) and guaranteeing the continuity of the environment construction.

[0124] Assuming sensor data is within the time interval The cubic spline interpolation function for (i=0,1,2,3,....n-1, where n is the number of sensor data frames) is as follows:

[0125] .

[0126] in, , , , These are the interpolation coefficients. The interpolation constraints require passing through the original data points, and their first derivative requires that the slopes of the interpolation curves in adjacent intervals be consistent at the junction points; their second derivative requires that the curvatures of the interpolation curves in adjacent intervals be consistent at the junction points.

[0127] The coefficients can be derived from these constraints: , , , .

[0128] in, This corresponds to the second derivative of the sensor data at the interpolation node. This means that the corresponding sensor data is at the [number]th [time]. Interpolation nodes (time) The second derivative at ) This indicates that the corresponding sensor data is at the [number]th [number]. Interpolation nodes (time) The second derivative at ().

[0129] Upper layer (data distribution layer): Retrieves synchronized data from the middle layer, performs validity checks to filter out erroneous or abnormal data, and then distributes it to multiple processing branches, providing reliable data information for subsequent modules such as real-time subgraph construction and global pose optimization. This requires calculating the average of historical N frames of synchronized data. and standard deviation ,in This is the data after synchronization of the i-th frame in history.

[0130] .

[0131] .

[0132] In this way, by combining dynamic delay compensation and cubic spline interpolation, the motion trajectory is fitted using continuous data from the inertial measurement unit. Then, based on the calculated average delay, the discrete data points of the lidar and odometer are interpolated to the precise moment of the reference time axis, generating a data sequence with perfectly aligned timestamps, thereby improving the time synchronization accuracy of multi-sensor data.

[0133] The raw point cloud data acquired by LiDAR is enormous (thousands of points per frame). Directly using it for scanning and matching local maps results in massive computational demands, impacting system real-time performance. To significantly reduce the amount of LiDAR point cloud data while preserving key environmental features (such as corners and table edges) and improve subsequent processing efficiency, a new approach is needed before scanning and matching the point cloud data with the current local sub-map to determine the robot's current pose. Figure 5 As shown, it also includes steps S510 to S530.

[0134] Step S510: Downsample and filter the input point cloud data using the initial maximum voxel size.

[0135] The original point cloud is filtered using a larger voxel size (e.g., 10cm × 10cm × 10cm). Based on the coordinate range of the laser point cloud data, a grid is divided according to the voxel size, and each voxel is assigned a unique identifier. In this way, the space is divided into a grid, and all points within each grid (voxel) are represented by their centroid.

[0136] For each non-empty voxel, calculate the average coordinates of all points in the point list, and use this average as the representative point of the voxel, adding it to the downsampled point cloud. Collect the representative points of all voxels to form simplified point cloud data, typically with 20% to 30% of the original point cloud data.

[0137] Based on the different performance of laser point clouds in open and simple scenes versus dense and complex scenes, two voxel filtering strategies were developed: fixed-size and adaptive. Fixed-size voxel filtering divides the 3D space into a uniform voxel grid at a preset resolution. For the input laser point cloud, leveraging the similar contribution of points within the same small volume to environmental description, a single point represents the features of the entire voxel, retaining only one point within each voxel. This reduces the amount of data while preserving key structures such as obstacle edges and corners, achieving downsampling through spatial deduplication. Adaptive voxel filtering addresses the issue of uneven point cloud density by dynamically adjusting the voxel size. It initially filters at the maximum resolution; if the resulting point cloud is too sparse, the voxel size is gradually reduced until the point cloud density meets a minimum point count threshold (e.g., 3), ensuring sufficient points to describe the shape in sparse areas and avoiding excessive retention of redundant points in dense areas.

[0138] Step S520: Determine whether the point cloud density of the downsampled point cloud data is lower than the preset minimum point count threshold.

[0139] Calculate the density of the filtered point cloud (e.g., the number of points per unit volume). If the density is lower than a preset minimum threshold, for example, ensuring that there are at least 3 points in each direction to describe a plane, then the point cloud is considered too sparse and there is a possibility of losing environmental features.

[0140] In step S530, if the point cloud density of the downsampled point cloud data is lower than the preset minimum point number threshold, the voxel size is reduced and downsampling filtering is performed again until the downsampled point cloud density is greater than or equal to the minimum point number threshold.

[0141] If the point cloud is too sparse, the voxel size is automatically reduced (e.g., to 5cm×5cm×5cm), and then the original point cloud is re-filtered using the new size. This process can be iterated until the density of the downsampled point cloud meets the requirements. Finally, the downsampled point cloud is scanned and matched with the current local sub-image to determine the robot's current pose.

[0142] Thus, this embodiment of the application uses an adaptive voxel filtering strategy to balance data volume and feature retention. It uses large voxels for efficient downsampling in open areas and automatically uses small voxels to retain details in complex areas, thereby improving processing speed and real-time performance while ensuring the accuracy of subsequent algorithms.

[0143] As robots build maps over long periods in large environments, local submaps grow continuously, leading to increased memory consumption and decreased loop closure detection efficiency. To manage the lifecycle of local submaps and balance map real-time performance, consistency, and system computational load, the following steps are implemented: In updating the current local submap based on the point cloud data of the current frame, the following steps are also included: continuously counting the number of frames of point cloud data used to update the current local submap; when the number of frames reaches a preset frame threshold, freezing the current local submap and storing it in the historical submap queue, and creating a new local submap to receive subsequent point cloud data. Mature submaps are marked as frozen and sent to the global map database for subsequent loop closure detection. Simultaneously, a new, empty local submap is immediately created to receive new data collected during subsequent robot movements.

[0144] In this way, the subgraph management mechanism enables modular map construction, which not only controls the size of individual subgraphs and improves computational efficiency, but also provides structured map data for backend optimization, which is an important guarantee for building large-scale, consistent environment maps.

[0145] When a robot is tracking a curved global path, it needs to know how much to turn in advance to smoothly navigate the curve. To translate the macroscopic direction of the global path into microscopic instructions guiding the robot's current local motion, the following steps are taken to extract look-ahead points on the global path based on the robot's current pose: starting from the matching point of the robot's current pose on the global path, accumulating the Euclidean distance between subsequent path points and generating a cumulative length; when the cumulative length reaches a preset look-ahead distance, the corresponding path point is designated as the look-ahead point, with the desired yaw angle being the azimuth angle of the line connecting the look-ahead point and the next path point.

[0146] It should be noted that the global path guidance and attitude optimization functions are implemented by constraining the robot's current attitude through the desired attitude of the global path, prioritizing the correction of yaw angle errors, and solving the attitude deviation problem caused by the lack of global guidance. First, a set of discrete path points for the global path is defined. ,in This is the k-th waypoint of the global path (based on the world coordinate system).

[0147] The robot's current pose is , Current position (For the current yaw angle), we first need to find the matching point of the current pose on the global path. (The nearest path point), then calculate the expected yaw angle θdes at that point (the tangent direction of the global path):

[0148] .

[0149] in = (That is, the currently matched global path point is the kth one). For the next path point on the global path, Ensure the yaw angle range is .

[0150] To achieve attitude priority, a higher weight needs to be assigned to yaw error correction in the allocation of robot control variables (linear velocity v, angular velocity ω). The objective function for the control variables is defined as a weighted sum of minimizing yaw error and minimizing position error, where the attitude weights... Much larger , .

[0151] in( , ) is the global path matching point The desired position. By minimizing the objective function J, the robot will first adjust the angular velocity ω to correct the yaw angle error, and then adjust the linear velocity v to correct the position error, thus achieving attitude priority.

[0152] From the current global path matching point Initially, accumulate the Euclidean distances of subsequent path points. When the accumulated length reaches the preset lookahead distance, the corresponding path point is designated as the lookahead point. If the lookahead distance is not reached by accumulating to the global path endpoint (f=m), then the lookahead point is the global path endpoint. This ensures the path is guided to the final destination. To ensure the local trajectory planning aligns with the global path direction, the yaw angle of the look-ahead point needs to be adjusted. As a trajectory direction constraint, the look-ahead yaw angle is defined as the direction of the line connecting the look-ahead point and subsequent path points.

[0153] .

[0154] In local trajectory planning, it is necessary to ensure the yaw angle at the trajectory endpoint. and The error satisfies:

[0155] .

[0156] in, The expected yaw angle for the look-ahead point. The lookahead points are extracted.

[0157] In this way, by introducing long-term geometric information of the global path into local control through the look-ahead mechanism, the robot can adjust its posture in advance, thereby producing smooth, human-like cornering behavior and avoiding violent shaking and sharp turns during movement.

[0158] In one implementation, during the long-term navigation of an indoor service robot, errors gradually accumulate in pose estimation and map building due to factors such as sensor noise, odometry drift, and environmental changes, leading to ghosting on the map or pose deviation from the true position. To address this issue, this patented solution introduces loop closure detection and pose graph optimization. Specifically, the point cloud data of the current frame is matched with subgraphs in the historical subgraph queue to perform loop closure detection; when a loop is detected, a pose graph containing the robot's historical pose nodes and constraint edges is constructed; the pose graph is then optimized to correct the robot's historical pose and global map.

[0159] The core steps include loop closure detection, pose graph construction, and pose graph optimization, thereby achieving global pose optimization. The core objective of global pose optimization is to construct a graph model composed of the robot's keyframe poses, minimize global pose error by mining loop constraints and fusing multi-sensor information, and ensure that the map is free of ghosting and the pose is free of drift after long-term motion.

[0160] The purpose of closed-loop detection is to identify whether the robot has returned to a previously visited area. The robot continuously maintains a history submap queue, which stores previously built and frozen local submaps, which represent the historical state of the environment.

[0161] For the point cloud data of the current frame, feature matching is performed between the point cloud data of the current frame and all subgraphs in the historical subgraph queue. The matching algorithm is usually based on scan matching or feature descriptors (such as point cloud feature similarity calculation). If the similarity between the current point cloud data and a certain historical subgraph exceeds a preset threshold (e.g., determined by correlation coefficient or geometric consistency), a loop closure is detected. For example, assuming that the matching error between the point cloud collected by the robot at a certain time point and the historical subgraph (corresponding time point) is less than the threshold, it is confirmed that the robot has returned to the region corresponding to the subgraph.

[0162] Among these methods, multi-sensor fusion combines data from LiDAR, odometry, and inertial measurement units, using filtering algorithms to leverage the advantages of multiple sensors to obtain a more accurate robot pose and motion state. Gravity alignment performs consistency calibration on the fused pose with other branch poses, ensuring uniformity of pose data from different sources. Examples include keyframe selection and single-sensor pose trajectory.

[0163] Keyframe selection aims to reduce computational load. Instead of fusing poses from every frame, keyframes are selected, and their poses serve as nodes in the global pose graph. Single-sensor pose estimation: The laser odometry pose directly uses the precise pose output from the front-end scan matching. The inertial data (acceleration, angular velocity) between two keyframes is pre-integrated to obtain the relative pose increment, which is then superimposed with the pose from the previous keyframe to obtain the current pose. The odometry pose is calculated based on the robot's base to obtain the current pose.

[0164] When a loop closure is detected, a pose graph is constructed to represent the robot's motion history and constraints: nodes in the pose graph represent the robot's keyframe poses. These keyframes are representative pose points selected from the robot's motion trajectory, typically based on time intervals or the degree of motion change. Each node contains the robot's position and orientation at that moment.

[0165] Edges represent the constraints between nodes, including odometry constraints, loop closure constraints, sensor fusion constraints, and graph structures. Odometry constraints are derived from the relative pose changes between adjacent keyframes (calculated using odometry or inertial data). Loop closure constraints add an edge between the current pose node and historical pose nodes when a loop closure is detected, indicating that they should be aligned (based on the relative pose calculated from the matching results). Sensor fusion constraints are pose estimates that fuse multi-sensor data (such as LiDAR, inertial measurement unit, and odometry), obtaining more accurate constraints through probabilistic weighted fusion.

[0166] Probabilistic weighted fusion is: for the three pose estimation results, weighted fusion is performed according to their noise covariance. , , Calculate weights The final fusion poses are fused using Lie algebra addition. .

[0167] in, , , These are robot pose estimates from LiDAR, inertial measurement unit, and odometry, respectively. , , The noise covariance matrix describes the uncertainty (noise magnitude and correlation) of robot pose estimation from LiDAR, inertial measurement unit, and odometry, respectively. is the determinant of the covariance matrix, used to measure the volume of the uncertainty ellipsoid described by the entire covariance matrix. The larger the value, the more uncertain and unreliable the sensor's current pose estimation is. As the fusion weight, it is assigned to the first... The weights of each sensor are calculated based on the core idea that a sensor's reliability is inversely proportional to the volume of its uncertainty. The weights are normalized so that the sum of the weights of all sensors is 1. The final and optimal pose estimation result is the fused pose.

[0168] The pose graph is a graph model (graph structure) in which nodes and edges jointly define the robot's trajectory and constraints. The optimization objective is to minimize the overall error of all constraints. Thus, the fused keyframe poses are used as nodes, and the relative poses of adjacent keyframes (calculated by inertial data pre-integration or laser odometry) are used as edges (constraint relationships) to construct the initial global pose graph.

[0169] After the pose graph is constructed, the optimal pose is solved by an optimization algorithm to correct the historical pose and the global map: the pose graph optimization is transformed into a nonlinear least squares problem.

[0170] The objective function minimizes the error of all constraint edges: .

[0171] in, Let be the set of all pose nodes. For nodes and Error function between (usually based on relative pose transformation) Let be the covariance matrix, representing the uncertainty of the constraints. The first one in the pose diagram Each pose node. The first one in the pose diagram Each pose node.

[0172] The solution is obtained using graph optimization libraries (such as g2o and Ceres Solver) or iterative algorithms (such as Gauss-Newton and Levenberg-Marquardt). The optimization process adjusts the poses of all nodes to ensure that loop closure constraints and odometry constraints are as consistent as possible, thereby eliminating accumulated errors. After optimization, the robot's entire historical trajectory and global map are adjusted to ensure map consistency (e.g., eliminating ghosting) and pose accuracy. For example, if the trajectory drifted before optimization, the optimized trajectory will align with the true path. Finally, loop closure detection compares the feature similarity between the current subgraph and historical subgraphs to identify whether the robot has circled back to previously traversed areas. For each subgraph, whether it is the baseline subgraph or a frozen historical subgraph, its geometric features are extracted, loop closure constraints are established, the pose nodes before and after the loop are connected, and corresponding constraint edges are added. The feature similarity between the current subgraph and all historical subgraphs is calculated. If the similarity exceeds a threshold, it is considered a loop closure. If a loop closure is detected, graph optimization can eliminate the accumulated pose errors from long-term motion, improving localization and mapping accuracy. This approach, through loop closure constraints, corrects pose drift and map distortion caused by long-term motion, improving navigation accuracy. Probabilistic weighted fusion leverages the strengths of various sensors to enhance the reliability of constraints. Keyframe selection and optimization algorithms ensure computational efficiency, making them suitable for real-time applications. Even when some sensors temporarily fail, the system can maintain stability using historical data.

[0173] This application also provides a service robot path planning device suitable for indoor environments, such as... Figure 6 As shown, it includes: an acquisition module, a local subgraph update module, a global path generation module, a lookahead point extraction module, and an execution trajectory determination module.

[0174] The acquisition module is used to acquire point cloud data from the lidar, inertial data from the inertial measurement unit, and displacement data from the odometer, and to synchronize the point cloud data, inertial data, and displacement data in time. The lidar, inertial measurement unit, and odometer are installed on the robot.

[0175] The local subgraph update module is used to scan and match point cloud data with the current local subgraph to determine the robot's current pose, and update the local subgraph based on the motion state filtering results.

[0176] The global path generation module is used to obtain the robot's starting point and target point, and determine the global path of the robot from the starting point to the target point based on the global map.

[0177] The look-ahead point extraction module is used to extract look-ahead points on the global path based on the robot's current pose and determine the expected yaw angle of the look-ahead points. The expected yaw angle is the target motion direction set for the robot based on the global path.

[0178] The execution trajectory determination module is used to determine the final execution trajectory based on the time-synchronized point cloud data, inertial data, displacement data, local sub-graph, global path and expected yaw angle. The final execution trajectory includes the linear velocity and angular velocity used to drive the robot's motion.

[0179] The above specific embodiments further illustrate the purpose, technical solution and beneficial effects of this application. It should be understood that the above are only specific embodiments of this application and are not intended to limit the scope of protection of this application. Any modifications, equivalent substitutions, improvements, etc., made on the basis of the technical solution of this application should be included within the scope of protection of this application.

Claims

1. A service robot path planning method suitable for indoor environments, characterized in that, include: The system acquires point cloud data from a lidar, inertial data from an inertial measurement unit (IMU), and displacement data from an odometer, and synchronizes the point cloud data, inertial data, and displacement data in time. The lidar, IMU, and odometer are mounted on the robot. The point cloud data is scanned and matched with the current local sub-graph to determine the robot's current pose, and the local sub-graph is updated based on the motion state filtering results. Obtain the robot's starting point and target point, and determine the robot's global path from the starting point to the target point based on the global map; Based on the robot's current pose, a look-ahead point is extracted on the global path, and the expected yaw angle of the look-ahead point is determined. The expected yaw angle is the target motion direction set for the robot based on the global path. Based on the time-synchronized point cloud data, the inertial data, the displacement data, the local subgraph, the global path, and the expected yaw angle, the final execution trajectory is determined, which includes the linear velocity and angular velocity used to drive the robot's motion. The step of determining the final execution trajectory based on the time-synchronized point cloud data, the inertial data, the displacement data, the local sub-map, the global path, and the desired yaw angle includes: Based on the time-synchronized point cloud data, the inertial data, and the displacement data, as well as the local sub-map, the global path, and the expected yaw angle, multiple predicted trajectories are sampled and simulated in the velocity space. Construct a cost map for evaluating the predicted trajectory, and set the dilation layer parameters of the cost map, wherein the dilation radius is... satisfy: ; The physical radius of the robot. This is the inertial gliding distance required for the robot to brake at the current speed. This is an additional safety margin. If the predicted trajectory intersects with a high-cost region in the cost map, the predicted trajectory is discarded. An evaluation function that includes the deviation between the trajectory endpoint yaw angle and the expected yaw angle is used to score multiple predicted trajectories, and the trajectory with the highest score is selected as the final execution trajectory.

2. The service robot path planning method suitable for indoor environments according to claim 1, characterized in that, The step of synchronizing the point cloud data, the inertial data, and the displacement data in time further includes: Analyze the timestamps of the point cloud data, the inertial data, and the displacement data; A reference time axis is established using the continuous high-frequency data stream of the inertial measurement unit, and the dynamic delay of the point cloud data and the displacement data relative to the reference time axis is determined. Based on the dynamic delay, cubic spline interpolation is used to align the point cloud data and the displacement data to the reference time axis to generate time-synchronized point cloud data, inertial data, and displacement data.

3. The service robot path planning method suitable for indoor environments according to claim 1, characterized in that, Before the step of scanning and matching the point cloud data with the current local sub-image to determine the robot's current pose, the method further includes: The input point cloud data is downsampled and filtered using the initial maximum voxel size; Determine whether the point cloud density of the downsampled point cloud data is lower than the preset minimum point count threshold; If the point cloud density of the downsampled point cloud data is lower than the preset minimum point number threshold, the voxel size is reduced and downsampling filtering is performed again until the downsampled point cloud density is greater than or equal to the minimum point number threshold.

4. The service robot path planning method suitable for indoor environments according to claim 1, characterized in that, The step of scanning and matching the point cloud data with the current local sub-image to determine the robot's current pose further includes: A global search is performed in a coarse-resolution likelihood field to obtain multiple candidate poses of the robot; Local optimization is performed on multiple candidate poses in a fine-resolution likelihood field to determine the optimal estimated pose among the multiple candidate poses as the current pose.

5. The service robot path planning method suitable for indoor environments according to claim 1, characterized in that, The step of updating the local subgraph based on the motion state filtering result includes: Calculate the deviation between the angular velocity in the inertial data and the angular velocity in the displacement data; Based on the comparison result between the deviation and the preset threshold, the motion state of the robot is selectively judged according to the displacement or the inertial data; If the motion state is determined to be a reliable motion state, the current local sub-graph is updated based on the point cloud data of the current frame; if the motion state is determined to be a stationary state or a low-speed motion state, the point cloud data of the current frame is discarded. The reliable motion state refers to the fact that the linear velocity and angular velocity of the robot both meet the preset conditions.

6. The service robot path planning method suitable for indoor environments according to claim 5, characterized in that, The step of updating the current local sub-graph based on the point cloud data of the current frame further includes: Continuously count the number of frames of point cloud data used to update the current local subgraph; When the number of frames reaches a preset frame threshold, the current local subgraph is frozen and stored in the historical subgraph queue, and a new local subgraph is created to receive subsequent point cloud data.

7. A service robot path planning method suitable for indoor environments according to claim 6, characterized in that, Also includes: The point cloud data of the current frame is matched with the subgraphs in the historical subgraph queue to perform loop closure detection; When a closed loop is detected, a pose graph containing the robot's historical pose nodes and constraint edges is constructed. The pose graph is optimized to correct the robot's historical pose and global map.

8. The service robot path planning method suitable for indoor environments according to claim 1, characterized in that, The step of extracting look-ahead points on the global path based on the robot's current pose includes: Starting from the matching point of the robot's current pose on the global path, accumulate the Euclidean distance between subsequent path points and generate the cumulative length; When the cumulative length reaches the preset look-ahead distance, the corresponding waypoint is determined as the look-ahead point, and the expected yaw angle is the azimuth angle of the line connecting the look-ahead point and the next waypoint.

Images