Service robot path planning method suitable for indoor environment

Abstract

The invention relates to the technical field of robots, and provides a service robot path planning method suitable for an indoor environment, and the method comprises the steps: obtaining the point cloud data of a laser radar, the inertial data of an inertial measurement unit, and the displacement data of an odometer, high-precision time synchronization, real-time mapping positioning, global path guidance and local obstacle avoidance decision are organically integrated together. Thus, the accuracy of environment perception is effectively improved, a reasonable driving path is planned based on the dynamic map and the position of the robot, the passing efficiency is ensured, collision with obstacles in a scene is avoided, the obstacle avoidance behavior has both safety and global directivity, and the obstacle avoidance efficiency is improved. And the navigation reliability, smoothness and intelligent level of the robot in a complex dynamic indoor environment are enhanced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a service robot path planning method suitable for indoor environment. BACKGROUND

[0002] With the rapid development of smart home, unmanned delivery and smart building applications, indoor service robots are increasingly popular, and their working environment is usually a complex unstructured scene containing dense furniture, dynamic pedestrians, narrow passages and other elements. In such an environment, the core prerequisite for the robot to achieve safe and efficient autonomous navigation is to rely on accurate and reasonable path planning.

[0003] Therefore, the robot is required to have the ability of environmental distance perception and real-time and reliable dynamic obstacle avoidance, that is, to construct a dynamic map consistent with the current environment in real time, and based on the dynamic map and the position of the robot, to plan a reasonable driving path, which not only ensures the traffic efficiency, but also avoids collision with obstacles in the scene. SUMMARY

[0004] The embodiment of the present application provides a service robot path planning method suitable for indoor environment, so as to plan a reasonable driving path for the service robot in the indoor environment, which not only ensures the traffic efficiency, but also avoids collision with obstacles in the scene.

[0005] The embodiment of the present application provides a service robot path planning method suitable for indoor environment, comprising: Obtaining point cloud data of a laser radar, inertial data of an inertial measurement unit and displacement data of an odometer, and time synchronizing the point cloud data, the inertial data and the displacement data, the laser radar, the inertial measurement unit and the odometer being arranged on the robot; Scanning and matching the point cloud data with a current local subgraph to determine a current pose of the robot, and updating the local subgraph based on a motion state filtering result; Obtaining a starting point and a target point of the robot, and determining a global path of the robot from the starting point to the target point based on a global map; Extracting a look-ahead point on the global path according to the current pose of the robot, and determining an expected yaw angle of the look-ahead point, the expected yaw angle being a target motion direction set for the robot based on the global path; Determining a final execution trajectory 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 including linear velocity and angular velocity for driving the motion of the robot.

[0006] In one implementation, in the step of determining the final execution trajectory 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, it comprises: Based on the time-synchronized point cloud data, inertial data and displacement data, as well as the local subgraph, global path and expected yaw angle, a plurality of predicted trajectories are generated by sampling and simulating in the velocity space; The plurality of predicted trajectories are scored by using an evaluation function containing the deviation of the trajectory end yaw angle from the expected yaw angle, and the trajectory with the highest score is selected as the final execution trajectory.

[0007] In an implementation, before the step of selecting the trajectory with the highest score as the final execution trajectory, the method further comprises: constructing a cost map for evaluating the predicted trajectory, and setting an inflation layer parameter of the cost map, wherein the inflation radius satisfies: ; the physical radius of the robot, the inertial sliding distance required for the robot to brake at the current speed, an additional safety margin; in the case where the predicted trajectory intersects with a high-cost region in the cost map, the predicted trajectory is removed.

[0008] In an implementation, the step of time-synchronizing the point cloud data, inertial data and displacement data further comprises: parsing the timestamps of the point cloud data, inertial data and displacement data; establishing a reference time axis based on the continuous high-frequency data stream of the inertial measurement unit, and determining the dynamic delay of the point cloud data and displacement data relative to the reference time axis; based on the dynamic delay, aligning the point cloud data and displacement data to the reference time axis by using a cubic spline interpolation method to generate time-synchronized point cloud data, inertial data and displacement data.

[0009] In an implementation, before the step of performing scan matching of the point cloud data with the current local subgraph to determine the current pose of the robot, the method further comprises: performing down-sampling filtering on the input point cloud data with an initial maximum voxel size; judging whether the point cloud density of the down-sampled point cloud data is lower than a preset minimum point threshold; in the case where the point cloud density of the down-sampled point cloud data is lower than the preset minimum point threshold, reducing the voxel size and performing down-sampling filtering again until the point cloud density after down-sampling is greater than or equal to the minimum point threshold.

[0010] In an implementation, in the step of performing scan matching of the point cloud data with the current local subgraph to determine the current pose of the robot, the method further comprises: performing a global search in the coarse-resolution likelihood field to obtain a plurality of candidate poses of the robot; performing a local optimization in the fine-resolution likelihood field on the plurality of candidate poses to determine an optimal estimated pose in the plurality of candidate poses as a current pose of the robot.

[0011] In an implementation, the step of updating the local submap based on the motion state filtering result comprises: calculating a deviation between an angular velocity in the inertial data and an angular velocity in the displacement data; selectively determining a motion state of the robot based on the displacement data or the inertial data based on a comparison result of the deviation and a preset threshold; in a case where the motion state is determined as a reliable motion state, updating the current local submap based on the point cloud data of the current frame; in a case where the motion state is determined as a static state or a low-speed motion state, discarding the point cloud data of the current frame, the reliable motion state being a state where both a linear velocity and an angular velocity of the robot satisfy a preset condition.

[0012] In an implementation, the step of updating the current local submap based on the point cloud data of the current frame further comprises: continuously counting a frame number of the point cloud data used for updating the current local submap; in a case where the frame number reaches a preset frame number threshold, freezing the current local submap and storing the current local submap into a history submap queue, and creating a new local submap for receiving subsequent point cloud data.

[0013] In an implementation, the method further comprises: matching the point cloud data of the current frame with submaps in the history submap queue to perform loop closure detection; when a loop closure is detected, constructing a pose graph comprising a history pose node of the robot and a constraint edge; optimizing and solving the pose graph to correct the history pose of the robot and a global map.

[0014] In an implementation, the step of extracting a look-ahead point on the global path based on the current pose of the robot comprises: starting from a matching point of the current pose of the robot on the global path, accumulating a Euclidean distance between subsequent path points, and generating an accumulated length; in a case where the accumulated length reaches a preset look-ahead distance, determining a corresponding path point as the look-ahead point, and determining an expected yaw angle as an azimuth angle of a line connecting the look-ahead point and a next path point.

[0015] The embodiment of the application provides a service robot path planning method suitable for an indoor environment, comprising 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 performing time synchronization on the point cloud data, the inertial data and the displacement data; the laser radar, the inertial measurement unit and the odometer are arranged on a robot; performing scanning matching on the point cloud data and a current local subgraph to determine a current pose of the robot, and updating the local subgraph based on a motion state filtering result; acquiring a starting point and a target point of the robot, and determining a global path of the robot from the starting point to the target point based on a global map; extracting a forward-looking point on the global path according to the current pose of the robot, and determining an expected yaw angle of the forward-looking point, wherein the expected yaw angle is a target motion direction set for the robot based on the global path; and determining a final execution trajectory 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, wherein the final execution trajectory comprises linear velocity and angular velocity used for driving the robot to move.

[0016] In this way, high-precision time synchronization, real-time mapping and positioning, global path guidance and local obstacle avoidance decision are organically integrated together. Therefore, 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, collision with obstacles in the scene is avoided, 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. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the application, the following will briefly introduce the drawings needed to be used in the embodiments. Obviously, for those skilled in the art, other drawings can also be obtained based on these drawings without any creative effort.

[0018] Figure 1 A flowchart of a service robot path planning method suitable for an indoor environment provided by the embodiment of the application Figure 1 Figure 2 A flowchart of a service robot path planning method suitable for an indoor environment provided by the embodiment of the application Figure 2 Figure 3 A flowchart of a service robot path planning method suitable for an indoor environment provided by the embodiment of the application Figure 3 Figure 4 A structural diagram of a service robot path planning method suitable for an indoor environment provided by the embodiment of the application Figure 4 .

[0019] ​​​Figure 5 A structural diagram of a service robot path planning method suitable for an indoor environment provided by an embodiment of the present application Figure 5 .

[0020] Figure 6 A structural diagram of a service robot path planning device suitable for an indoor environment provided by an embodiment of the present application DETAILED DESCRIPTION

[0021] The embodiments will be described in detail below with reference to the drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following embodiments are not meant to represent all implementations consistent with the present application. Rather, they are merely examples of systems and methods consistent with some aspects of the present application as detailed in the appended claims.

[0022] It should be noted that the brief description of the terms in the present application is only for the convenience of understanding the following described embodiments, and is not intended to limit the embodiments of the present application. Unless otherwise specified, these terms should be understood according to their ordinary and general meanings.

[0023] The terms first, second, third and the like in the specification and claims of the present application and the above-described drawings are used to distinguish similar or like objects or entities, and do not necessarily mean a specific order or sequence, unless otherwise noted. It should be understood that the terms used in this way can be interchanged under appropriate circumstances.

[0024] The terms include and have and any variations thereof are intended to cover, but not exclusively, inclusion, for example, a product or device including a series of components need not be limited to all components clearly listed, but can include other components not clearly listed or inherent to such products or devices.

[0025] The term module refers to any known or later developed hardware, software, firmware, artificial intelligence, fuzzy logic, or combination of hardware or / and software code capable of performing a function associated with that element.

[0026] Indoor service robots (such as cleaning robots, delivery robots) perform tasks in complex environments such as homes, offices, etc. There are dense furniture, dynamic walking personnel and narrow passages in the environment. When the robot navigates autonomously in such a dynamic and complex environment, it needs to plan a reasonable path to ensure efficient passage and avoid collision with obstacles in the scene.

[0027] To solve the above technical problems, an embodiment of the present application provides a service robot path planning method suitable for an indoor environment, as shown in Figure 1 S110-S150.

[0028] In step S110, the point cloud data of the laser radar, the inertial data of the inertial measurement unit, and the displacement data of the odometer are acquired, and the point cloud data, the inertial data, and the displacement data are time-synchronized.

[0029] The laser radar, the inertial measurement unit, and the odometer are arranged on the robot; the robot acquires the point cloud data, the inertial data, and the displacement data of the environment through the laser radar, the inertial measurement unit, and the odometer carried by the robot.

[0030] The laser radar is a sensor that acquires three-dimensional information of the environment through laser ranging and dynamic scanning. It emits near-infrared laser beams at a fixed frequency, and the laser beams are guided by a scanning mechanism to cover the surrounding space at a certain angle. When the laser beam encounters an obstacle, part of the light will be reflected, and the reflected light will be captured by a photoreceptor. Then the continuously collected point cloud is real-time spliced and denoised, and a three-dimensional contour model of the surrounding environment is constructed, clearly presenting the position, shape, distance, and other information of the obstacle, providing accurate environmental perception data for robot navigation, autonomous driving, and other scenarios.

[0031] The inertial measurement unit (IMU) is a sensor that senses motion and attitude changes through internal sensitive elements and acquires real-time motion state of the 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 attitude angle, motion trajectory, and speed information of the device in real time.

[0032] The odometer monitors the displacement change of the motion component of the device, and the raw displacement data collected needs to be calibrated for error to finally output the instantaneous speed, cumulative travel distance, and coordinates relative to the starting point in real time.

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

[0034] In step S120, the point cloud data is scanned and matched with the current local subgraph to determine the current pose of the robot, and the local subgraph is updated based on the motion state filtering result.

[0035] In order to realize local map construction and robot positioning, the time-synchronized laser point cloud is used to match the current point cloud with the constructed local subgraph through a scanning matching algorithm to accurately calculate the position and orientation of the robot in the current subgraph, i.e. the current pose of the robot.

[0036] Specifically, a likelihood field related scan matching algorithm (Correlative Scan Matching, CSM) is used for scan matching. The CSM algorithm solves the scan matching problem by constructing a coarse resolution and fine resolution likelihood field model. First, a global search is performed in the coarse resolution likelihood field. By dividing the coarse resolution likelihood field search space into a low resolution grid, the matching error of the pose corresponding to each grid vertex is calculated, and the first N candidate poses with the smallest error are retained, i.e. a plurality of candidate poses of the robot are obtained. Then, a more detailed search is performed on the grid related to the best pose in the fine resolution likelihood field, and local optimization is performed on the plurality of candidate poses to obtain the optimal pose at a higher resolution, i.e. the optimal estimated pose among the plurality of candidate poses is determined as the current pose. Through this step-by-step search process, the CSM algorithm can accurately estimate the position of the robot at different resolutions, achieving efficient scan matching.

[0037] In the embodiments of the present application, when the robot performs scan matching, it is necessary to quickly and accurately find the best matching pose of the current point cloud and the map within a large range of position and angle uncertainty. In order to solve the contradiction between the large calculation amount of global search and the easy falling into local optimum of local search in scan matching. First, a global search is performed in the coarse resolution likelihood field. The search range is large, covering all areas where the robot may exist, and a plurality of candidate poses of the robot are obtained. Then, in the neighborhood of the several best candidate poses obtained in the previous step, a more detailed search is performed using a high resolution likelihood field map. Through iterative optimization, an optimal estimated pose is finally determined from the candidate poses. In this way, through the two-stage search strategy from coarse to fine, combining the robustness of global search and the accuracy of local search. It can not only avoid falling into a local optimal solution, but also ensure the calculation efficiency, realize real-time and accurate positioning.

[0038] Wherein, when constructing the environment map, it is determined whether to update the local subgraph with the current data according to the real-time motion state of the robot (such as whether it is stationary), and specifically, as shown in Figure 2 The method comprises steps S121 to S123.

[0039] In step S121, the deviation between the angular velocity in the inertial data and the angular velocity in the displacement data is calculated.

[0040] When the robot is running on a smooth ground, wheel slip may occur. In this case, there is a possibility of misalignment when determining the motion state of the robot only by the displacement data of the odometer. Therefore, the present application calculates the deviation between the angular velocity provided by the inertial measurement unit and the angular velocity provided by the odometer in real time.

[0041] Step S122, based on the comparison result of the deviation and the preset threshold, selectively judging the motion state of the robot according to the displacement or the inertial data.

[0042] In the case that the deviation between the angular velocity provided by the inertial measurement unit and the angular velocity provided by the odometer is lower than the preset first threshold, the motion state is judged based on the linear velocity and the angular velocity provided by the displacement data of the odometer.

[0043] In the case that the deviation is higher than or equal to the first threshold, it is considered that the wheeled structure of the odometer may slip, and the measurement data is not reliable. The motion state of the robot is judged based on the angular velocity provided by the inertial data of the inertial measurement unit.

[0044] Step S123, in the case that the motion state is judged as the reliable motion state, the current local subgraph is updated according to the point cloud data of the current frame; in the case that the motion state is judged as the static state or the low-speed motion state, the point cloud data of the current frame is abandoned.

[0045] The reliable motion state means that the linear velocity and the angular velocity of the robot both satisfy the preset condition. For example, the motion state is judged based on the linear velocity and the angular velocity provided by the displacement data of the odometer, which means that in the case that the absolute value of the linear velocity and the absolute value of the angular velocity provided by the odometer are both higher than the corresponding preset threshold, it is determined that the robot is in the reliable motion state. In the case that the absolute value of the linear velocity and the absolute value of the angular velocity are both lower than the corresponding preset threshold, it is determined that the robot is in the static or low-speed state.

[0046] For another example, the motion state is judged based on the inertial data, which means that in the case that the absolute value of the angular velocity provided by the inertial data is higher than or equal to the corresponding preset threshold, it is determined that the robot is in the motion state; in the case that the absolute value of the angular velocity provided by the inertial data is lower than the corresponding preset threshold, it is determined that the robot is in the static or low-speed state. The specific value of the preset threshold is set by the technician according to experience, or is set according to the specific scene, for example, in the scene where there is only equipment or both equipment and personnel, the size of the preset threshold is different.

[0047] The embodiment of the application filters reliable data and dynamically updates the current local map through motion filtering and subgraph management, avoids noise pollution of the map, and ensures local consistency of the subgraph. When the robot is stationary or moving at low speed, the pose estimation error of scan matching will significantly increase (sensor noise ratio rises). At this time, the current laser scan frame is discarded through speed threshold judgment to avoid inserting noise points into the map and ensure the cleanliness of the subgraph. The subgraph is the basic unit of the local map, an initial reference subgraph is created, the laser point cloud filtered by motion filtering is continuously inserted, the grid probability is iteratively updated using the Bayesian formula, and the occupancy probability of the corresponding grid is updated according to whether the point falls on the obstacle position each time the laser point cloud is added, so as to repeatedly iterate and construct the environmental features.

[0048] When the number of scan frames accumulated by the subgraph reaches the threshold, the new reference subgraph is no longer updated and created. These subgraphs serve as historical references for subsequent matching with new subgraphs (supporting closed-loop detection), while the subgraphs 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 subgraph in the form of a voxel grid (each grid is marked with whether there is an obstacle or not), and the local environmental map around the robot is constructed or updated in real time.

[0049] In this way, the scheme improves the robustness of motion state judgment through cross verification of the inertial measurement unit and the odometer. It can effectively identify and handle the odometer failure, ensure that the mapping process remains stable even under non-ideal conditions, and avoid pollution of the map by false data.

[0050] Step S130, the starting point and target point of the robot are obtained, and the global path of the robot from the starting point to the target point is determined based on the global map.

[0051] On the known global map, a global optimal path from the starting point to the target point is planned according to the target point given by the user.

[0052] Step S140, a look-ahead point is extracted on the global path according to the current pose of the robot, and an 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.

[0053] According to the current pose of the robot, a look-ahead point at a certain distance in front on the global path is selected, and the direction of the point (expected yaw angle) is calculated as the motion direction guide of the robot. The expected yaw angle is the tangent direction of the look-ahead point on the global path. Specifically, the expected yaw angle is determined according to the direction of the line segment between the look-ahead point and one or more subsequent path points on the global path.

[0054] Step S150, determining a final execution trajectory based on the time-synchronized point cloud data, inertial data, displacement data, local subgraph, global path and expected yaw angle.

[0055] wherein the final execution trajectory comprises linear velocity and angular velocity for driving the robot to move.

[0056] In this way, the embodiments of the present application organically integrate high-precision time synchronization, real-time mapping and positioning, global path guidance and local obstacle avoidance decision-making together. Thus, the accuracy of environment perception is effectively improved, and a reasonable driving path is planned based on the dynamic map and the position of the robot, which not only ensures the traffic efficiency, but also avoids collision with obstacles in the scene, 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.

[0057] In the process of robot walking, the robot moves along the global path, and dynamic obstacles (such as pedestrians or vehicles) may suddenly appear on the path. At this time, a new local trajectory needs to be planned immediately to control the robot to detour. As shown in Figure 3 The embodiments of the present application include steps S310 to S320.

[0058] Step S310, based on the time-synchronized point cloud data, inertial data and displacement data, and the local subgraph, global path and expected yaw angle, sampling and simulating a plurality of predicted trajectories in a velocity space.

[0059] Specifically, according to the current speed and acceleration of the robot, the linear velocity and angular velocity in the future are combined and sampled within a dynamic window. For each group of speed and acceleration, the kinematic model of the robot is used to simulate and predict the motion trajectory in the next few seconds, thereby generating a plurality of candidate predicted trajectories.

[0060] Step S320, using an evaluation function containing the deviation of the trajectory endpoint yaw angle and the expected yaw angle to score the plurality of predicted trajectories, and selecting the trajectory with the highest score as the final execution trajectory.

[0061] Specifically, for each candidate trajectory, an evaluation function is used to score, and the trajectory with the highest score is selected as the final execution trajectory. The core evaluation index of the evaluation function is the deviation of the robot orientation (yaw angle) at the trajectory endpoint and the expected yaw angle. The smaller the deviation, the higher the score. This ensures that the robot will select those trajectories that can align the direction of its body to the direction of the global path.

[0062] The expression of the evaluation function J is: .

[0063] wherein, and to predict the lateral and longitudinal position deviation of the trajectory endpoint from the lookahead point, to predict the yaw angle deviation of the trajectory endpoint from the desired yaw angle, , , are corresponding weight coefficients; wherein the weight coefficient is much larger than the weight coefficient and , so that the evaluation function gives priority to the predicted trajectory with smaller yaw angle deviation. In this way, the direction of the global path (through the desired yaw angle) is directly integrated into the local obstacle avoidance decision, which can effectively avoid the robot deviating from the global direction due to local obstacle avoidance, even falling into a deadlock, significantly improving the intelligence and success rate of navigation.

[0064] In actual application process, the robot needs to pass near the obstacle (such as table leg, wall), which needs to ensure that no collision occurs with the obstacle, and also needs to maintain a reasonable safety distance from the obstacle.

[0065] In order to ensure the safety of the final execution trajectory, before the step of selecting the trajectory with the highest score as the final execution trajectory, a cost map for evaluating the predicted trajectory is constructed, and the inflation layer parameter of the cost map is set.

[0066] Specifically, first, a cost map is generated according to the real-time local subgraph. Each grid in the map is set with a cost value, and the cost value of the region where the obstacle is located is the highest. Then the obstacle is inflated. In actual application process, the inflation radius is not fixed, but dynamically calculated.

[0067] The inflation radius of the cost map needs to cover the physical size of the robot, the inertia braking distance, and the operation safety margin, and its inflation radius satisfies: . Wherein, is the physical radius of the robot, is the inertia sliding distance required for the robot to brake at the current speed, is an additional safety margin to ensure that the inflated high-cost area can cover all potential collision risks. In this way, the setting can ensure that the inflation area can completely cover the outline of the robot and the braking risk area.

[0068] In order to make the cost in the inflation layer decay gently with the increase of the distance from the obstacle, and only the middle area is a low-cost area.

[0069] .

[0070] If represents close to the obstacle, it is set to high cost, and if it is represents a gentle decay, if represents no risk, set to the lowest cost.

[0071] where, is the distance, which refers to the Euclidean distance from the current grid (a small cell in the map) to the nearest obstacle. R_min is the inscribed radius (distance threshold), when represents that the grid is located inside the obstacle or has invaded the absolute safety distance of the robot. is the maximum cost value, which refers to the upper limit of the cost value, when the function directly returns . is the decay coefficient, which is a parameter for controlling the decay speed of the cost value, the greater the value, the slower the decay of the cost value with the increase of the distance , that is, the greater the range of the high-cost area and the gentler the transition. Conversely, the smaller, the faster the decay, and the steeper the transition from the safe area to the dangerous area. is the minimum cost value, which refers to the lower limit of the cost value, when tends to infinity, the exponential term tends to 0, so tends to 0. But in practical applications, a (typically 0) will be set as the cost of the free space.

[0072] Before trajectory evaluation, all candidate trajectories are compared with the cost map. In the case of intersection between the predicted trajectory and the high-cost area in the cost map, the predicted trajectory is removed, that is, if any point on a trajectory falls in the inflated area with a cost value higher than the safety threshold, the trajectory is determined as a dangerous trajectory and is directly removed from the candidate list.

[0073] In this way, the application establishes an efficient safety screening mechanism by introducing a cost map considering the robot dynamics and safety margin. It can exclude all unsafe options in the early stage of trajectory planning, ensuring that the subsequent scoring process is only performed in the set of safe trajectories, thereby fundamentally avoiding collision risks and improving the safety of the system. The hardware components and software modules in the application constitute a robot to realize the navigation function.

[0074] In practical applications, the collection frequencies of the laser radar, the inertial measurement unit and the odometer may be different. For example, the collection frequency of the laser radar is 10 Hz, the collection frequency of the inertial measurement unit is 100 Hz, and the collection frequency of the odometer is 20 Hz. If the collected data is directly used, it will cause ghosting in environment perception or positioning drift.

[0075] In the step of time synchronizing the point cloud data, the inertial data and the displacement data, as shown in Figure 4 , the step S410 to the step S430 are further included.

[0076] In the step S410, the time stamps of the point cloud data, the inertial data and the displacement data are parsed.

[0077] In the step S420, a reference time axis is established based on the continuous high-frequency data stream of the inertial measurement unit, and a dynamic delay of the point cloud data and the displacement data relative to the reference time axis is determined.

[0078] A reasonable time deviation threshold value is set based on the current system time, and a valid time interval is defined. .

[0079] Wherein is the current system time, is the maximum allowed time deviation threshold value, and the time stamp of each frame of sensor data (point cloud data, inertial data, displacement data odometer) is checked. If it does not belong to the interval, it is marked and the abnormal data is removed.

[0080] The continuous high-frequency data stream of the inertial measurement unit is taken as the reference time axis, and a time synchronization coordinate system is established. The delay deviation of the point cloud data time stamp of the laser radar relative to the reference axis is calculated. Specifically, the step of calculating the dynamic delay includes: calculating the difference between the time stamp of each frame of data of the laser radar and the odometer and the corresponding time on the reference time axis in real time, to obtain the instantaneous delay . In order to further smooth the fluctuation, the average value of these delays is calculated using a sliding window (such as the last 20 frames) , as the current dynamic compensation amount.

[0081] For example, a reasonable time deviation threshold value is set based on the sensor frame rate and the communication delay, and a valid time interval of the time stamp is defined. .

[0082] Wherein is the current system time (based on the inertial measurement unit), is the maximum allowed time deviation threshold value, and the time stamp of each frame of sensor data is checked. If it does not belong to the interval, it is marked and the abnormal data is removed.

[0083] Wherein, the dynamic delay is determined based on the time stamp of the inertial measurement unit. Specifically, the delay model of the kth frame of a certain sensor (such as a laser radar) is as follows: .

[0084] Take the last N frames (window size N is determined by the sensor frame rate) to calculate the dynamic compensation amount, every 1 frame of data is added, the window slides 1 time, and the update As follows: .

[0085] Wherein is the timestamp of the kth frame of data of the inertial measurement unit, is the timestamp of the kth frame of data of the sensor (such as laser radar), is the delay of the kth frame of the sensor relative to the inertial measurement unit, is the size of the sliding window, is the dynamic average delay (compensation amount) of the kth frame.

[0086] That is, a fixed size sliding time window is maintained, which contains recent multiple frames of point cloud data and corresponding inertial data and displacement data; In the sliding time window, the deviation of the timestamp of each frame of point cloud data from the corresponding time on the reference axis is calculated; The statistical results of all the deviations in the sliding time window are analyzed as the current dynamic delay deviation estimate. Then, based on the delay deviation, the timestamps of the point cloud data and the displacement data are dynamically compensated to align with the reference axis.

[0087] Step S430, based on the dynamic delay, the point cloud data and the displacement data are aligned to the reference time axis by using the cubic spline interpolation method, to generate time-synchronized point cloud data, inertial data and displacement data.

[0088] Dynamic timestamp compensation is performed, taking the timestamp of the inertial measurement unit as the reference time axis, calculating the dynamic delay average of the laser radar and the odometer through the sliding window, performing spline interpolation on the continuous data, and ensuring the smoothness of the interpolation curve through the second derivative of the adjacent data points, avoiding the jump of the laser point cloud data due to interpolation, and ensuring the continuity of the surrounding environment construction. Then generate data that completely match the target timestamp, update the compensation amount in real time, so as to realize the correct mapping of the laser radar and the odometer timestamp to the axis.

[0089] Data time alignment interpolates the sensor discrete data to each target time point of the reference time axis of the inertial measurement unit through cubic spline interpolation, ensures the continuity (smoothness without jump) of the second derivative of the interpolation curve, and ensures the continuity of the environment construction.

[0090] Suppose the sensor data is in the time interval The cubic spline interpolation function of (i=0,1,2,3,....n-1,n is the frame number of sensor data) is as follows: .

[0091] Wherein, , , , are the interpolation coefficients. The interpolation constraints require that the original data points are passed through, the first derivative of the interpolation curve needs to be consistent with the slope of the adjacent interval at the junction, and the second derivative of the interpolation curve needs to be consistent with the curvature of the adjacent interval at the junction.

[0092] The coefficients can be derived through these constraints: , , , .

[0093] wherein, is the second derivative of the corresponding sensor data at the interpolation node, is the second derivative of the corresponding sensor data at the first interpolation node (time ), is the second derivative of the corresponding sensor data at the second interpolation node (time ).

[0094] Upper layer (data distribution layer): obtains the synchronized data from the middle layer, first performs validity check on the data, filters the error or abnormal data, and then distributes to multiple processing branches, to provide reliable data information for subsequent real-time subgraph construction and global pose optimization modules. The mean value and the standard deviation of the historical N frames of synchronized data need to be calculated, wherein is the historical i-th frame of synchronized data.

[0095] .

[0096] .

[0097] In this way, by means of the combination of dynamic delay compensation and cubic spline interpolation, the motion trajectory is fitted by using the continuous data of the inertial measurement unit, then the discrete data points of the laser radar and the odometry are interpolated to the accurate time of the reference time axis according to the calculated average delay, to generate a data sequence with completely aligned timestamps, and the time synchronization accuracy of the multi-sensor data is improved.

[0098] The raw point cloud data collected by the laser radar is huge (thousands of points per frame), and directly used for scanning matching of the local map, which has a huge amount of calculation and affects the real-time performance of the system. In order to greatly reduce the data amount of the laser point cloud under the premise of preserving the key features of the environment (such as corners and table edges), so as to improve the subsequent processing efficiency. Before the step of scanning matching the point cloud data with the current local subgraph to determine the current pose of the robot, such as Figure 5As shown, it also includes steps S510 to S530.

[0099] Step S510, down-sampling filtering the input point cloud data with an initial maximum voxel size.

[0100] Filter the original point cloud using a larger voxel size (e.g. 10cm x 10cm x 10cm). According to the coordinate range of the laser point cloud data, divide the grid according to the voxel size, and set a unique identifier for each voxel. In this way, the space is divided into grids, and all points in each grid (voxel) are represented by a center point.

[0101] For each non-empty voxel, calculate the average coordinates of all points in the point list, and add the average value as the representative point of the voxel to the down-sampled point cloud. Collect the representative points of all voxels to form the simplified point cloud data, and the number of points is usually 20% to 30% of the original point cloud data.

[0102] According to the different performances of laser point cloud in open simple and dense complex scenes, two voxel filtering strategies of fixed size and self-adaption are developed. Fixed size voxel filtering is to divide the three-dimensional space into uniform voxel grid according to the preset resolution. For the input laser point cloud, the points in the same small volume contribute similar characteristics to the environment description, and a single point represents the characteristics of the entire voxel. Only one point is retained in each voxel, which reduces the amount of data while retaining the key structures such as the edges and corners of obstacles. Through spatial deduplication, down-sampling is achieved. Adaptive voxel filtering is to dynamically adjust the voxel size to address the problem of uneven point cloud density. First, filter with the maximum resolution. If the resulting point cloud is too sparse, gradually reduce the voxel size until the point cloud density meets the minimum point threshold, for example, the minimum point threshold is 3, to ensure that the sparse area still has enough points to describe the shape, and the dense area does not retain redundant points.

[0103] Step S520, judge whether the point cloud density of the down-sampled point cloud data is lower than the preset minimum point threshold.

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

[0105] Step S530, in the case where the point cloud density of the down-sampled point cloud data is lower than the preset minimum point threshold, reduce the voxel size and re-perform the down-sampling filtering until the point cloud density after down-sampling is greater than or equal to the minimum point threshold.

[0106] If the point cloud is too sparse, the voxel size is automatically reduced (e.g., to 5cmx5cmx5cm), and the original point cloud is then filtered again with the new size. This process can be iterated until the density of the down-sampled point cloud meets the requirements. Finally, the down-sampled point cloud is scanned and matched with the current local submap to determine the current pose of the robot.

[0107] In this way, the embodiment of the present application balances the data volume and feature retention degree through the adaptive voxel filtering strategy, efficiently down-samples in the open area with large voxels, and automatically retains details in the complex area with small voxels, thereby improving the processing speed and real-time performance while ensuring the accuracy of subsequent algorithms.

[0108] The robot builds a map in a large-scale environment for a long time, and the local submap will continue to grow, resulting in increased memory usage and decreased loop detection efficiency. In order to manage the life cycle of the local submap and balance the real-time performance, consistency and computational load of the system, in the step of updating the current local submap according to the point cloud data of the current frame, the frame number of the point cloud data used to update the current local submap is continuously counted; in the case where the frame number reaches a preset frame number threshold, the current local submap is frozen and stored in the historical submap queue, and a new local submap is created to receive subsequent point cloud data. The current mature submap is marked as frozen and sent to the global map database for subsequent loop detection. At the same time, a new empty local submap is immediately created to receive new data collected during the subsequent motion of the robot.

[0109] In this way, the submap management mechanism makes the map construction modular, controls the size of a single submap, improves the computational efficiency, and provides structured map data for backend optimization, which is an important guarantee for building a large-scale and consistent environment map.

[0110] When the robot tracks a curved global path, it needs to know how much to turn in advance to smoothly pass the bend. In order to convert the macroscopic direction of the global path into micro-instructions to guide the current local motion of the robot, the step of extracting a look-ahead point on the global path according to the current pose of the robot includes: starting from the matching point of the current pose of the robot on the global path, accumulating the Euclidean distance between subsequent path points, and generating an accumulated length; in the case where the accumulated length reaches a preset look-ahead distance, the corresponding path point 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 path point.

[0111] It should be noted that the implementation of the global path guidance and pose optimization function is to constrain the current pose of the robot by the expected pose of the global path, and to preferentially correct the yaw angle error to solve the pose deviation problem caused by no global guidance. First, define a discrete path point set of the global path wherein The kth waypoint of the global path (based on the world coordinate system).

[0112] The current pose of the robot is , The current position is The current yaw angle is ), the matching point of the current pose on the global path (the nearest path point) needs to be found first, and then the expected yaw angle θdes of the point (the tangent direction of the global path) is calculated: .

[0113] Where = (the current matching global path point is the kth), is the next path point on the global path, to ensure that the yaw angle range is .

[0114] To achieve pose priority, the yaw angle error correction needs to be given a higher weight in the robot control variable (linear velocity v, angular velocity ω) allocation. The target function of the control variable is defined as the weighted sum of the minimum yaw angle error and the minimum position error, where the attitude weight is much larger than , .

[0115] Where , is the position (expected position) of the global path matching point . By minimizing the target function J, the robot will preferentially adjust the angular velocity ω to correct the yaw angle error, and then adjust the linear velocity v to correct the position error, achieving pose priority.

[0116] Starting from the current global path matching point , the Euclidean distance of the subsequent path points is accumulated, and when the accumulated length reaches the preset lookahead distance, the corresponding path point is the lookahead point . If the accumulated distance does not reach the lookahead distance by the end of the global path (f=m), the lookahead point is the end of the global path , ensuring that the final target is reached. To make the local trajectory planning fit the direction of the global path, the yaw angle of the lookahead point needs to be used as a trajectory direction constraint. The lookahead point yaw angle is defined as the direction of the line connecting the lookahead point and the subsequent path point.

[0117] .

[0118] In local trajectory planning, the error between the yaw angle of the trajectory endpoint and needs to be ensured to satisfy: .

[0119] wherein, is the expected yaw angle of the look-ahead point, is the extracted look-ahead point.

[0120] In this way, by the look-ahead point mechanism, the long-term geometric information of the global path is introduced into the local control, enabling the robot to adjust the pose in advance, thereby producing smooth and human-like behavior when turning, avoiding violent shaking and sharp turns in motion.

[0121] In an implementation, in the long-term navigation of an indoor service robot, due to factors such as sensor noise, odometry drift and environmental changes, the pose estimation and map construction of the robot will gradually accumulate errors, causing the map to have ghosting or the pose to deviate from the true position. To solve this problem, the patent scheme introduces loop closure detection and pose graph optimization to solve the above problems. Specifically, the point cloud data of the current frame is matched with the subgraphs in the history subgraph queue to perform loop closure detection; when a loop is detected, a pose graph containing the historical pose nodes and constraint edges of the robot is constructed; the pose graph is optimized to correct the historical pose of the robot and the global map.

[0122] That is, the core steps include loop closure detection, pose graph construction and pose graph optimization, thereby realizing global pose optimization. The core goal of global pose optimization is to construct a graph model composed of key frame poses of the robot, to minimize the global pose error by mining loop constraints and fusing multi-sensor information, and to ensure that the map has no ghosting and the pose has no drift after long-term motion.

[0123] wherein, the purpose of loop closure detection is to identify whether the robot has returned to a previously visited area. The robot continuously maintains a history subgraph queue (History Submap Queue), which stores previously constructed and frozen local subgraphs (Frozen Local Submaps). These subgraphs represent the historical state of the environment.

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

[0125] Wherein, multi-sensor fusion combines the data of laser radar, odometer, inertial measurement unit, uses filtering algorithm, fuses the advantages of multiple sensors to get more accurate robot pose and motion state, and gravity alignment is the consistency calibration of the pose after multi-sensor fusion and other branch poses, to ensure the unity of pose data from different sources. For example, key frame selection and single sensor pose trajectory.

[0126] Key frame selection is to reduce the amount of calculation, without fusing each frame of pose, but selecting key frames, and the key frame pose is as the node of global pose graph. Single sensor pose estimation: laser odometer pose directly uses the accurate pose output by front-end scan matching, pre-integrates the inertial data (acceleration, angular velocity) between two key frames to get relative pose increment, and superimposes the previous key frame pose to get the current pose; odometer pose is calculated based on the robot base to get the current pose.

[0127] When a loop closure is detected, a pose graph is constructed to represent the motion history and constraint relationship of the robot: the nodes in the pose graph represent the key frame poses of the robot. These key frames are representative pose points selected from the robot motion trajectory, usually based on time interval or motion change degree. Each node contains the position and direction of the robot at that time.

[0128] Edges represent the constraint relationship between nodes, including: odometer constraint, loop constraint, sensor fusion constraint and graph structure. Odometer constraint is the relative pose change between adjacent key frames (calculated through odometer or inertial data). Loop constraint is when a loop closure is detected, an edge is added between the current pose node and the historical pose node, indicating that they should be aligned (the relative pose calculated based on the matching result). Sensor fusion constraint is the pose estimation of fusing multiple sensor data (such as laser radar, inertial measurement unit, odometer), which gets more accurate constraint through probability weighted fusion.

[0129] Probability weighted fusion is: for three pose estimation results, according to their noise covariance , , , calculate the weight , and finally fuse the pose according to Lie algebra addition fusion .

[0130] Among them, , , are the robot pose estimations from laser radar, inertial measurement unit, odometer respectively. , , is the noise covariance matrix, describing the uncertainty (noise size and correlation) of the robot pose estimate from the laser, IMU, and odometry, respectively. is the determinant of the covariance matrix, measuring the volume of the uncertainty ellipsoid described by the whole covariance matrix, The larger the value, the more uncertain and unreliable the current pose estimate of the sensor is. is the fusion weight, assigned to the th sensor, which is computed based on a core idea: the reliability of a sensor is inversely proportional to the volume of its uncertainty. The weights are normalized so that the sum of all sensor weights is 1. is the fused pose, the final and optimal pose estimate.

[0131] Pose graph is a graph model (graph structure), in which nodes and edges jointly define the trajectory and constraints of the robot, and the optimization goal is to minimize the overall error of all constraints. Thus, the fused keyframe poses are taken as nodes, and the relative poses between adjacent keyframes (computed from IMU pre-integration or laser odometry) are taken as edges (constraints), to construct the initial global pose graph.

[0132] After the pose graph is constructed, an optimization algorithm is used to solve the optimal pose to correct the historical poses and global map: pose graph optimization is converted into a nonlinear least squares problem.

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

[0134] where, is the set of all pose nodes, is the error function (usually based on the relative pose transformation) between nodes and is the covariance matrix, representing the uncertainty of the constraint, represents the th pose node in the pose graph. represents the th pose node in the pose graph.

[0135] ​Solve by using graph optimization library (such as g2o, Ceres Solver) or iterative algorithm (such as Gauss-Newton, Levenberg-Marquardt). The optimization process adjusts the pose of all nodes so that the loop constraints and odometry constraints are as consistent as possible, thereby eliminating the cumulative error. After optimization, the entire historical trajectory of the robot and the global map are adjusted to ensure the consistency of the map (such as eliminating ghosting) and the accuracy of the pose. For example, if the trajectory before optimization has drift, the trajectory after optimization will align with the true path. Finally, through loop detection, the similarity of the features of the current subgraph and the historical subgraph is compared to identify whether the robot has circled the area it has passed through before. For each subgraph, whether it is a reference subgraph or a frozen historical subgraph, its geometric features are extracted, loop constraints are established, the pose nodes before and after the loop are connected, and the corresponding constraint edges are added. Calculate the feature similarity of the current subgraph and all historical subgraphs, if the similarity exceeds the threshold, it is determined to be a closed loop, if a closed loop is detected, the cumulative pose error of long-term motion can be eliminated through graph optimization to improve the accuracy of positioning and mapping. Through loop constraints, the pose drift and map distortion caused by long-term motion are corrected, and the navigation accuracy is improved. By using probabilistic weighted fusion, the advantages of various sensors are integrated to enhance the reliability of the constraints. The key frame selection and optimization algorithm ensures the computational efficiency and is suitable for real-time applications. Even when some sensors are temporarily disabled, the system can remain stable through historical data.

[0136] The embodiment of the present application also provides a path planning device for a service robot in an indoor environment, as shown in the drawings, comprising an acquisition module, a local subgraph updating module, a global path generation module, a look-ahead point extraction module and an execution trajectory determination module. Figure 6

[0137] The acquisition module is used to acquire point cloud data of a laser radar, inertial data of an inertial measurement unit and displacement data of an odometer, and to time synchronize the point cloud data, the inertial data and the displacement data, the laser radar, the inertial measurement unit and the odometer being arranged on the robot.

[0138] The local subgraph updating module is used to perform scan matching on the point cloud data and a current local subgraph to determine a current pose of the robot, and to update the local subgraph based on a motion state filtering result.

[0139] The global path generation module is used to acquire a start point and a target point of the robot, and to determine a global path of the robot from the start point to the target point based on a global map.

[0140] The look-ahead point extraction module is used to extract a look-ahead point on the global path according to the current pose of the robot, and to determine an expected yaw angle of the look-ahead point, the expected yaw angle being a target motion direction set for the robot based on the global path.

[0141] ​An execution trajectory determination module is configured to determine a final execution trajectory based on the time-synchronized point cloud data, the inertial data, the displacement data, the local submap, the global path, and the desired yaw angle, the final execution trajectory including linear and angular velocities for driving motion of the robot.

[0142] The above detailed description is further to the purpose, technical solutions and beneficial effects of the present application. It should be understood that the above is only a specific embodiment of the present application and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made on the basis of the technical solutions of the present application shall be included in the protection scope of the present application.

Claims

1. A service robot path planning method suitable for indoor environments, characterized in that, The method 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 time synchronizing the point cloud data, the inertial data and the displacement data, wherein the laser radar, the inertial measurement unit and the odometer are arranged on a robot; performing scan matching on the point cloud data and a current local subgraph to determine a current pose of the robot, and updating the local subgraph based on a motion state filtering result; acquiring a starting point and a target point of the robot, and determining a global path of the robot from the starting point to the target point based on a global map; extracting a forward-looking point on the global path according to the current pose of the robot, and determining an expected yaw angle of the forward-looking point, wherein the expected yaw angle is a 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, determining a final execution trajectory, wherein the final execution trajectory comprises linear velocity and angular velocity for driving motion of the robot. 2.The path planning method for a service robot suitable for an indoor environment according to claim 1, wherein, In the step of determining the final execution trajectory 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 method comprises the following steps: based on the time-synchronized point cloud data, the inertial data and the displacement data, and the local subgraph, the global path and the expected yaw angle, sampling and simulating a plurality of predicted trajectories in a velocity space; using an evaluation function comprising a deviation of a trajectory end point yaw angle from the expected yaw angle to score the plurality of predicted trajectories, and selecting a trajectory with the highest score as the final execution trajectory. 3.The method of claim 2, wherein, Before the step of selecting the trajectory with the highest score as the final execution trajectory, the method further comprises the following step: constructing a cost map for evaluating the predicted trajectory, and setting an inflation layer parameter of the cost map, wherein an inflation radius satisfies: ; is a physical radius of the robot, is an inertia slide distance required for the robot to brake at the current speed, is an additional safety margin set. in the case that the predicted trajectory intersects with a high-cost region in the cost map, eliminating the predicted trajectory. 4.The path planning method for a service robot suitable for an indoor environment according to claim 1, wherein, In the step of time synchronizing the point cloud data, the inertial data and the displacement data, the method further comprises the following steps: analyzing time stamps of the point cloud data, the inertial data and the displacement data; establishing a reference time axis with a continuous high-frequency data stream of the inertial measurement unit, and determining a dynamic delay of the point cloud data and the displacement data relative to the reference time axis; based on the dynamic delay, using a cubic spline interpolation method 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.

5. The path planning method for a service robot suitable for indoor environments according to claim 1, wherein, Before the step of performing scan matching on the point cloud data and a current local subgraph to determine a current pose of the robot, the method further comprises the following steps: performing down-sampling filtering on the input point cloud data with an initial maximum voxel size; judging whether the point cloud density of the down-sampled point cloud data is lower than a preset minimum point threshold; in the case that the point cloud density of the down-sampled point cloud data is lower than the preset minimum point threshold, reducing the voxel size and re-performing down-sampling filtering until the point cloud density of the down-sampled point cloud data is greater than or equal to the minimum point threshold. 6.The path planning method for a service robot suitable for an indoor environment according to claim 1, wherein, In the step of scanning matching the point cloud data with the current local submap to determine the current pose of the robot, further comprising: performing a global search in a coarse resolution likelihood field to obtain a plurality of candidate poses of the robot; performing a local optimization in a fine resolution likelihood field to the plurality of candidate poses to determine an optimal estimated pose in the plurality of candidate poses as the current pose.

7. The path planning method for a service robot suitable for indoor environments according to claim 1, wherein, In the step of updating the local submap based on the motion state filtering result, comprising: calculating a deviation between an angular velocity in the inertial data and an angular velocity in the displacement data; selectively judging the motion state of the robot according to the displacement data or the inertial data based on a comparison result of the deviation and a preset threshold; in a case that the motion state is determined as a reliable motion state, updating the current local submap according to the point cloud data of the current frame; in a case that the motion state is determined as a static state or a low-speed motion state, discarding the point cloud data of the current frame, the reliable motion state refers to that both the linear velocity and the angular velocity of the robot satisfy a preset condition. 8.The method of claim 7, wherein, In the step of updating the current local submap according to the point cloud data of the current frame, further comprising: continuously counting and updating a frame number of the point cloud data used for the current local submap; in a case that the frame number reaches a preset frame number threshold, freezing the current local submap and storing the current local submap into a historical submap queue, and creating a new local submap for receiving subsequent point cloud data. 9.The method of claim 8, wherein, Further comprising: matching the point cloud data of the current frame with the submaps in the historical submap queue to perform loop closure detection; when detecting a loop closure, constructing a pose graph comprising historical pose nodes and constraint edges of the robot; optimizing and solving the pose graph to correct the historical poses of the robot and a global map. 10.The path planning method for a service robot suitable for an indoor environment according to claim 1, wherein, The step of extracting a look-ahead point on the global path according to the current pose of the robot, comprising: starting from a matching point of the current pose of the robot on the global path, accumulating the Euclidean distances between subsequent path points, and generating an accumulated length; in a case that the accumulated length reaches a preset look-ahead distance, determining a corresponding path point as the look-ahead point, the expected yaw angle is a bearing angle of a line connecting the look-ahead point and a next path point.

Images