A robot autonomous path planning and obstacle avoidance control method

Abstract

The application relates to the technical field of mobile robots and discloses a robot autonomous path planning and obstacle avoidance control method, which comprises the following steps: S1, an environment modeling and kinematic calibration stage; S2, a global optimal path pre-planning stage; S3, a real-time environment perception and obstacle information processing stage; S4, a local dynamic obstacle avoidance and path re-planning stage; S5, a trajectory tracking and bottom layer motion control stage; and S6, a safety bottom-up and exception processing closed loop stage. Through the hierarchical linkage architecture of global path planning and local dynamic obstacle avoidance, the application realizes the deep fusion of global task guidance and local dynamic obstacle avoidance, generates a smooth reference path conforming to the motion constraints of the robot through an optimized global path planning algorithm, guarantees the directionality and stability of global task execution, realizes flexible obstacle avoidance in a dynamic scene through a local obstacle avoidance algorithm with global guidance, and avoids the problem that the robot deviates from the global route during the obstacle avoidance process.

Description

Technical Field

[0001] This invention relates to the field of mobile robot technology, and in particular to a method for autonomous path planning and obstacle avoidance control of robots. Background Technology

[0002] With the rapid development of mobile robot technology, autonomous navigation capability has become a core performance indicator of mobile robots. Autonomous path planning and obstacle avoidance control technology are the core key technologies for realizing the autonomous, safe and stable operation of mobile robots from the starting point to the target point, which directly determines the robot's operation efficiency, operational safety and scene adaptability. In various practical application scenarios, the working environment often contains both fixed static obstacles and randomly appearing dynamic obstacles, which places high demands on the rationality of the robot's path planning, dynamic obstacle avoidance response capability, and global task execution stability. How to achieve efficient linkage between global path planning and local dynamic obstacle avoidance, while ensuring the safety of robot operation and the accuracy of task execution, has become a key research and optimization direction in this field. Existing robot path planning and obstacle avoidance control technologies mostly adopt an architecture where global planning and local obstacle avoidance are independent of each other. There is a lack of effective linkage constraints between the two, which can easily lead to problems such as the robot deviating from the global planning path and deviating from the mission route during local obstacle avoidance. Summary of the Invention

[0003] The purpose of this invention is to provide a method for autonomous path planning and obstacle avoidance control of robots to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, the present invention adopts the following technical solution: A method for autonomous path planning and obstacle avoidance control of a robot includes the following steps: S1. Environmental modeling and kinematic calibration stage: Collect global data of the work scene and robot motion data through multi-source sensors, construct a 2D grid map of the work scene, complete map preprocessing and calibration of core robot kinematic parameters, and determine the basic constraints for path planning. S2, Global Optimal Path Pre-planning Stage: Analyze the starting point, ending point, and global constraints of the task input, complete the global path search and path smoothing, generate a global reference path that conforms to the robot's motion constraints, and distribute it. S3, Real-time environmental perception and obstacle information processing stage: Simultaneously collect real-time data from multiple sensors, complete data denoising preprocessing, realize obstacle classification and dynamic obstacle motion trajectory prediction, and update the local rolling grid map. S4, Local Dynamic Obstacle Avoidance and Path Replanning Stage: Guided by the global reference path, a local rolling window is constructed to complete the selection of feasible speed windows and the evaluation and selection of the optimal trajectory, generating local obstacle avoidance control commands, and triggering emergency replanning when the path is blocked. S5, Trajectory Tracking and Low-Level Motion Control Stage: In the stage of trajectory tracking and low-level motion control, the robot's real-time pose is calculated, the optimal control quantity for trajectory tracking is solved, control commands are sent to the motor drive actuator, and the actual operation data of the robot is collected to complete the closed-loop feedback. S6. Safety backup and anomaly handling closed-loop stage: Monitor the robot's operating status and safe distance throughout the process, identify abnormal operating states and trigger corresponding handling strategies, and complete target point pose verification and task closure.

[0005] As a further improvement to this technical solution: S1 specifically includes the following steps: synchronously acquiring point cloud data of the entire operation scene and robot displacement, velocity, and attitude data through the lidar acquisition device, the wheeled odometer device, the IMU inertial measurement device, and the ultrasonic blind spot acquisition device; generating a 2D grid map of the operation scene through the global map construction submodule; performing obstacle expansion processing on the grid map through the map preprocessing submodule; dividing the absolutely infeasible region, restricted feasible region, and free feasible region; and calibrating the robot's linear velocity, angular velocity, minimum turning radius, and maximum acceleration / deceleration threshold through the kinematic calibration submodule.

[0006] As a further improvement to this technical solution: S2 specifically includes the following steps: extracting the global constraints of the task input, including the starting point, ending point, prohibited routes, priority channels, maximum path length, and task time limit, through the task instruction parsing submodule; and completing the global path search through the improved A* algorithm global path planning submodule. The path search adopts a multi-objective cost function, the expression of which is:

[0007] in This represents the total cost of path search. The cost is the actual path length from the starting point to the current node. Estimate the cost of the Manhattan distance from the current node to the target point. The additional constraint cost includes the cost of the number of turns, the cost of channel priority, and the cost of safety distance. The inflection point fitting of the global path is completed by the cubic B-spline path smoothing submodule, the path curvature continuity processing is completed, and a global reference path that conforms to the robot motion constraints is generated. The generated global reference path is then sent to the local planning stage.

[0008] As a further improvement to this technical solution, S3 specifically includes the following steps: The multi-sensor synchronous acquisition submodule completes the time synchronization and spatial calibration of multi-source data from the lidar, the ultrasonic blind spot acquisition device, the wheeled odometer, and the IMU; the point cloud data denoising submodule completes the pass-through filtering and statistical filtering of the lidar point cloud data, eliminating abnormal data and environmental noise; the obstacle classification and state update submodule completes the classification of static and dynamic obstacles, real-time updates of static obstacles on the local scrolling grid map, fitting of the position, speed, and direction of motion of dynamic obstacles, and prediction of the future trajectory and occupied area of ​​dynamic obstacles.

[0009] As a further improvement to this technical solution: S4 specifically includes the following steps: The local scrolling window construction submodule constructs a local scrolling window centered on the robot's current pose; the feasible velocity window filtering submodule generates feasible windows for linear velocity and angular velocity based on the robot's kinematic constraints and current motion state, eliminating invalid velocity combinations that would lead to collisions or exceed hardware thresholds; the globally guided DWA local planning submodule completes trajectory evaluation and optimal instruction selection. The trajectory evaluation uses a multi-dimensional evaluation function, the function expression of which is:

[0010] in The overall score for trajectory evaluation. This represents the deviation between the simulated trajectory and the global reference path. To simulate the minimum distance between the trajectory and the obstacle, For the smoothness parameter of velocity change, The parameter representing the fit between the simulated trajectory and the target point. , , , The adjustable weighting coefficients for each evaluation index are used; the speed combination with the highest score is selected as the control command for the current cycle; when the global reference path is completely blocked or the local planning has no feasible solution, the obstacle avoidance local path is generated through the EB-RRT* emergency replanning submodule, and after the obstacle avoidance is completed, the robot is controlled to return to the global reference path.

[0011] As a further improvement to this technical solution, S5 specifically includes the following steps: The pose fusion calculation submodule fuses the data from the wheel odometer and the IMU to calculate the robot's real-time pose; the MPC model prediction control submodule takes the desired trajectory output from local planning as input and combines it with the robot's kinematic model to solve for the optimal control quantity for trajectory tracking; the optimal control quantity is then sent to the motor drive actuator to control the robot to complete the corresponding motion; and the motion state feedback submodule collects the robot's actual operating data in real time and feeds it back to the positioning, planning, and control links to complete closed-loop control.

[0012] As a further improvement to this technical solution, S6 specifically includes the following steps: The multi-level safety monitoring submodule monitors the minimum distance between the robot and obstacles, its motion state, sensor status, and positioning accuracy throughout the process. It sets two safety thresholds: a first-level warning threshold and a second-level braking threshold. When the first-level warning threshold is triggered, it performs deceleration and obstacle avoidance strategy adjustments. When the second-level braking threshold is triggered, it performs an emergency stop. The abnormal state identification submodule identifies abnormal states such as abnormal sensor data, continuous lack of feasible planned paths, and path deviation exceeding thresholds, triggering corresponding degradation processing strategies. The emergency braking execution device completes the robot's emergency stop when the second-level braking threshold is triggered. The task closed-loop verification submodule performs pose verification after the robot reaches the target point, confirming task completion and awaiting the next task instruction.

[0013] Compared with the prior art, the beneficial effects of the present invention are: 1. This invention achieves deep integration of global task guidance and local dynamic obstacle avoidance through a hierarchical linkage architecture of global path planning and local dynamic obstacle avoidance. It generates a smooth reference path that conforms to the robot's motion constraints through an optimized global path planning algorithm, ensuring the directionality and stability of global task execution. It also achieves flexible obstacle avoidance in dynamic scenarios through a local obstacle avoidance algorithm with global guidance, avoiding the problem of the robot deviating from the global path during obstacle avoidance. At the same time, it achieves advance obstacle avoidance through dynamic obstacle trajectory prediction, which greatly improves the smoothness of the robot's obstacle avoidance action and its adaptability to dynamic scenarios.

[0014] 2. This invention constructs a closed-loop control chain covering the entire process from environmental perception, path planning, trajectory tracking to safety safeguards, ensuring the accurate implementation of planning instructions and the safety of the robot's operation throughout the process. The comprehensive hierarchical safety monitoring and anomaly handling mechanism significantly improves the robot's operational stability and reliability when facing complex working conditions and abnormal scenarios. The overall architecture of the solution is clear, highly interpretable, and easy to deploy and debug, making it adaptable to the usage requirements of various structured operation scenarios and effectively improving the efficiency and operational safety of mobile robots' autonomous operation.

[0015] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it according to the contents of the specification, the preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings. Specific embodiments of the present invention are given in detail below with reference to the accompanying drawings. Attached Figure Description

[0016] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings: Figure 1 This is a schematic diagram of the structure of a robot autonomous path planning and obstacle avoidance control method. Detailed Implementation

[0017] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are for illustrative purposes only and are not intended to limit the scope of the invention. The invention is described more specifically in the following paragraphs by way of example with reference to the accompanying drawings. It should be noted that the drawings are in a very simplified form and use non-precise proportions, and are only used to facilitate and clarify the illustration of the embodiments of the present invention.

[0018] Please see Figure 1 In this embodiment of the invention, a robot autonomous path planning and obstacle avoidance control method includes the following steps: S1. Environmental modeling and kinematic calibration stage: Collect global data of the work scene and robot motion data through multi-source sensors, construct a 2D grid map of the work scene, complete map preprocessing and calibration of core robot kinematic parameters, and determine the basic constraints for path planning. S2, Global Optimal Path Pre-planning Stage: Analyze the starting point, ending point, and global constraints of the task input, complete the global path search and path smoothing, generate a global reference path that conforms to the robot's motion constraints, and distribute it. S3, Real-time environmental perception and obstacle information processing stage: Simultaneously collect real-time data from multiple sensors, complete data denoising preprocessing, realize obstacle classification and dynamic obstacle motion trajectory prediction, and update the local rolling grid map. S4, Local Dynamic Obstacle Avoidance and Path Replanning Stage: Guided by the global reference path, a local rolling window is constructed to complete the selection of feasible speed windows and the evaluation and selection of the optimal trajectory, generating local obstacle avoidance control commands, and triggering emergency replanning when the path is blocked. S5, Trajectory Tracking and Low-Level Motion Control Stage: In the stage of trajectory tracking and low-level motion control, the robot's real-time pose is calculated, the optimal control quantity for trajectory tracking is solved, control commands are sent to the motor drive actuator, and the actual operation data of the robot is collected to complete the closed-loop feedback. S6. Safety backup and anomaly handling closed-loop stage: Monitor the robot's operating status and safe distance throughout the process, identify abnormal operating status and trigger corresponding handling strategies, and complete target point pose verification and task closure. Specifically, the S1 environment modeling and kinematic calibration stage provides the basic environmental digital twin and robot body motion constraints for the entire method. It is the premise for all subsequent path planning and control actions, ensuring that the planned path matches the robot's actual motion capabilities, and at the same time, constructing an accurate digital foundation for the work scenario. The S2 global optimal path pre-planning stage provides a core reference benchmark for the robot's full-process navigation. It generates a globally optimal path framework based on task requirements, preventing the robot from deviating from the task objective during dynamic obstacle avoidance and ensuring the directionality and integrity of the navigation task. The S3 real-time environmental perception and obstacle information processing stage provides real-time and accurate environmental data input for dynamic obstacle avoidance, enabling comprehensive perception and prediction of static and dynamic obstacles in the work scene. It is the core data support link for the robot to achieve dynamic obstacle avoidance. The S4 local dynamic obstacle avoidance and path replanning stage is one of the core execution links of the method. Guided by the global path and combined with real-time environmental data, it generates local obstacle avoidance control commands that are adapted to the dynamic scene. At the same time, it sets up an emergency replanning mechanism to deal with extreme scenarios where the global path is blocked, ensuring the continuity and safety of robot operation. The S5 trajectory tracking and underlying motion control stage is the execution stage that transforms planning instructions into the actual movement of the robot, achieving accurate tracking of the planned path. At the same time, through a closed-loop feedback mechanism, it eliminates the deviation between the actual movement of the robot and the planning instructions, ensuring the accurate implementation of the planning results. The S6 safety fallback and anomaly handling closed-loop stage provides safety assurance for the robot's entire operation, covering handling strategies for various anomaly scenarios, while completing closed-loop verification of the task to ensure the operational safety of the entire method and the integrity of task execution.

[0019] S1 specifically includes the following steps: synchronously collecting point cloud data of the entire operation scene and robot displacement, velocity, and attitude data through a lidar acquisition device, a wheeled odometer device, an IMU inertial measurement device, and an ultrasonic blind spot acquisition device; generating a 2D grid map of the operation scene through a global map construction submodule; performing obstacle expansion processing on the grid map through a map preprocessing submodule; dividing the absolutely infeasible region, restricted feasible region, and free feasible region; and calibrating the robot's linear velocity, angular velocity, minimum turning radius, and maximum acceleration / deceleration threshold through a kinematic calibration submodule. Specifically, the lidar acquisition device is used to collect full-domain 3D point cloud data of the work scene, obtain the position and contour information of all objects in the scene, and provide core original scene data for global map construction; The wheel-type odometer device is used to collect wheel-end rotation speed data during robot operation, calculate robot displacement and linear velocity information, and provide basic data for robot motion state monitoring and kinematic calibration. An IMU (Inertial Measurement Unit) is used to collect three-axis acceleration and three-axis angular velocity data of a robot, calculate the robot's real-time attitude and angular acceleration information, and provide basic data for attitude dimension for robot motion state monitoring and kinematic calibration. The ultrasonic blind spot acquisition device is used to collect obstacle data in the robot's near blind spots, making up for the detection blind spots of the lidar in the near range, and ensuring that the scene data acquisition has no blind spots. The global map construction submodule is used to receive scene data from multi-source acquisition devices, and complete the construction of a 2D grid map of the work scene through the SLAM algorithm, transforming the physical scene into a digital map that can be recognized and processed by the path planning algorithm. The map preprocessing submodule is used to optimize the constructed 2D grid map, expand obstacles based on the robot's body size, and divide the absolutely infeasible region, restricted feasible region, and free feasible region according to the scene's traffic rules. This provides clear traffic boundary constraints for path planning and avoids the risk of collision caused by the planned path conforming to obstacles. The kinematic calibration submodule is used to calibrate the core motion parameters of the robot, complete the calibration of the robot's linear velocity and angular velocity control accuracy, and define the robot's minimum turning radius and maximum acceleration / deceleration thresholds. This provides clear hardware constraints for subsequent path planning and speed control, ensuring that the planned path and speed commands conform to the robot's actual motion capabilities.

[0020] S2 specifically includes the following steps: Extracting the global constraints of the task input—starting point, ending point, prohibited routes, priority paths, maximum path length, and task time limit—through the task instruction parsing submodule; and completing the global path search through the improved A* algorithm global path planning submodule. The path search uses a multi-objective cost function, the expression of which is:

[0021] in This represents the total cost of path search. The cost is the actual path length from the starting point to the current node. Estimate the cost of the Manhattan distance from the current node to the target point. The additional constraint cost includes the cost of the number of turns, the cost of channel priority, and the cost of safety distance. The inflection point fitting of the global path is completed by the cubic B-spline path smoothing submodule, the path curvature continuity processing is completed, and a global reference path that conforms to the robot motion constraints is generated. The generated global reference path is then sent to the local planning stage. Specifically, the task instruction parsing submodule is used to receive task instructions from the host computer or human-computer interaction terminal; extract the start and end point information from the instructions; and parse the global constraints corresponding to the task, including prohibited routes, priority channels, maximum path length, and task time limit; providing clear task objectives and constraint boundaries for global path search.

[0022] An improved global path planning submodule of the A algorithm is used to perform global path search and calculation; it is the core computational unit of this stage; based on the parsed task start point, end point and constraints, it uses a multi-objective cost function to complete the traversal and search of path nodes; and generates the initial global path.

[0023] The multi-objective cost function expression used in this submodule is as follows: The formula serves as the core calculation basis for global path search; it quantifies the comprehensive cost of each path node; the algorithm iterates through all feasible nodes; selects the node with the minimum total cost F to complete the path search; and finally generates the globally optimal path. Detailed annotations and functions of each parameter in the formula are as follows: The total cost of the path search is the core basis for the algorithm to determine the priority of nodes; the lower the total cost, the higher the priority of the corresponding node, and the more likely it is to be included in the planned path. This represents the actual path length cost from the starting point to the current node; it's used to quantify the length of the path already traversed from the task's starting point to the current traversed node; the longer the path length, the greater the cost. The higher the value, the higher the total cost of the corresponding node. It estimates the cost of the Manhattan distance from the current node to the target point; it quantifies the estimated path length from the current traversed node to the task endpoint; it is a heuristic function of the A algorithm; it ensures that the search direction of the algorithm always points towards the task endpoint; it avoids invalid global traversal; and it improves path search efficiency. This parameter represents an additional constraint cost; used to adapt to various global constraints of the task; including turning number cost, channel priority cost, and safety distance cost. The turning number cost is used to constrain the number of inflection points on the path, reducing the robot's turning actions and improving operational smoothness. The channel priority cost is used to guide the path to prioritize the use of preset priority channels and avoid prohibited routes. The safety distance cost is used to constrain the distance between the path and obstacles, ensuring that the planned path always maintains a preset safety margin with respect to obstacles. This parameter solves the shortcomings of the traditional A algorithm, such as the path closely following obstacles, too many inflection points, and failure to meet task constraints.

[0024] The cubic B-spline path smoothing submodule is used to optimize the initial polyline path generated by the improved A algorithm; fit the inflection points of the path using the cubic B-spline interpolation algorithm; achieve continuous processing of path curvature; eliminate right-angle inflection points and sharp turns in the initial path; ensure that the smoothed path meets the robot's minimum turning radius and maximum acceleration / deceleration constraints throughout; generate a global reference path that can be actually executed by the robot; and simultaneously distribute the generated global reference path to the subsequent local planning stage; providing a core guidance benchmark for local obstacle avoidance.

[0025] S3 specifically includes the following steps: The multi-sensor synchronous acquisition submodule completes the time synchronization and spatial calibration of multi-source data from LiDAR, ultrasonic blind spot acquisition device, wheeled odometer, and IMU; the point cloud data denoising submodule completes the pass-through filtering and statistical filtering of laser point cloud data, and removes abnormal data and environmental noise; the obstacle classification and status update submodule completes the classification of static and dynamic obstacles, real-time updates of static obstacles on the local scrolling grid map, fitting of the position, speed, and direction of motion of dynamic obstacles, and prediction of the future trajectory and occupied area of ​​dynamic obstacles. Specifically, the multi-sensor synchronous acquisition submodule is used to complete the time synchronization and spatial calibration of multi-source sensor data. The time synchronization is used to unify the data acquisition timestamps of LiDAR, ultrasonic blind spot acquisition device, wheel odometry and IMU, and eliminate the data delay deviation of different sensors. The spatial calibration is used to unify the coordinate system of all sensors, and uniformly map the data collected by different sensors to the robot body coordinate system, ensuring the spatial consistency of multi-source data and providing a unified data source for subsequent environmental perception. The point cloud data denoising submodule is used to preprocess the real-time point cloud data collected by LiDAR. It removes invalid point cloud data outside the scene through pass-through filtering and removes noise data caused by environmental reflection and sensor anomalies through statistical filtering, so as to ensure the accuracy of obstacle detection and avoid false detection and false obstacle avoidance caused by noise data. The obstacle classification and state update submodule is the core processing unit in this stage. It is used to identify and classify obstacles in the preprocessed point cloud data. First, the detected obstacles are divided into static obstacles and dynamic obstacles. For static obstacles, their position information is updated in real time to the local rolling grid map to provide static environment boundaries for local path planning. For dynamic obstacles, the extended Kalman filter algorithm is used to fit the position, velocity, and direction of motion of the obstacle. At the same time, the motion trajectory and occupied area of ​​the dynamic obstacle in the future multiple control cycles are predicted, providing a predictive basis for local obstacle avoidance. This enables early obstacle avoidance rather than passive emergency obstacle avoidance, improving the smoothness and safety of the robot's obstacle avoidance actions.

[0026] S4 specifically includes the following steps: Constructing a sub-module using a local scrolling window, centered on the robot's current pose; filtering the sub-module using feasible velocity windows, generating feasible windows for linear and angular velocities based on robot kinematic constraints and the current motion state; eliminating invalid velocity combinations that could lead to collisions or exceed hardware thresholds; and completing trajectory evaluation and optimal instruction selection through a globally guided DWA local planning sub-module. The trajectory evaluation uses a multi-dimensional evaluation function, the expression of which is:

[0027] in The overall score for trajectory evaluation. This represents the deviation between the simulated trajectory and the global reference path. To simulate the minimum distance between the trajectory and the obstacle, For the smoothness parameter of velocity change, The parameter representing the fit between the simulated trajectory and the target point. , , , Adjustable weighting coefficients for each evaluation index; select the speed combination with the highest score as the control command for the current cycle; when the global reference path is completely blocked or the local planning has no feasible solution, generate a local obstacle avoidance path through the EB-RRT* emergency replanning submodule, and control the robot to return to the global reference path after the obstacle avoidance is completed; Specifically, the local scrolling window construction submodule is used to construct a local scrolling window with a preset radius centered on the robot's current pose. This limits the calculation range of local path planning to within this window, significantly reducing the computational load of the algorithm and improving the real-time performance of local planning. At the same time, the window scrolls and updates in real time as the robot moves, ensuring that it always covers the core environmental area around the robot. The feasible velocity window filtering submodule is used to generate feasible windows for linear velocity and angular velocity based on the robot's kinematic constraints and current motion state. At the same time, it traverses all velocity combinations, eliminates velocity combinations that will cause the robot to collide with obstacles, and invalid velocity combinations that exceed the maximum threshold of the robot's hardware, and retains only feasible velocity combinations that meet safety requirements and hardware constraints, providing effective input for subsequent trajectory evaluation. The globally guided DWA local planning submodule is the core computing unit in this stage. It is used to simulate and generate the robot's motion trajectory in the next control cycle for each combination of velocities within the feasible velocity window. At the same time, it uses a multi-dimensional evaluation function to quantify and score all simulated trajectories, and selects the velocity combination corresponding to the trajectory with the highest score as the robot control command for the current control cycle. The multi-dimensional evaluation function expression used in this submodule is as follows: The formula is the core calculation basis for local trajectory optimization, used to quantify the comprehensive adaptability of each simulated trajectory. The algorithm calculates the comprehensive evaluation score of all feasible trajectories and selects the trajectory with the highest score as the optimal execution trajectory to ensure that the local obstacle avoidance action meets the safety requirements and does not deviate from the global mission route. The detailed annotations and functions of each parameter in the formula are as follows: The comprehensive evaluation score of the trajectory is the core basis for the algorithm to judge the quality of the trajectory. The higher the score, the better the comprehensive fit of the corresponding trajectory, and the higher the priority it is to be selected as the execution trajectory. The deviation value between the simulated trajectory and the global reference path is used to quantify the degree of deviation between the local obstacle avoidance trajectory and the global planning baseline path. The smaller the deviation value, the higher the contribution of this item to the score, ensuring that the robot always stays close to the global reference path during obstacle avoidance and avoids deviating from the mission route. The minimum distance between the simulated trajectory and obstacles is used to quantify the safety level of the trajectory. The larger the minimum distance, the higher the contribution of this item to the score, ensuring that the robot's movement trajectory always maintains a safe distance from obstacles and avoids the risk of collision. The speed change smoothness parameter is used to quantify the magnitude of the change in the robot's speed command. The smaller the magnitude of the speed change, the higher the contribution of this item to the score, ensuring that the robot's acceleration and deceleration actions are smooth, avoiding sudden stops and turns, and improving the smoothness of operation. The parameter representing the fit of the simulated trajectory toward the target point is used to quantify the degree of fit between the trajectory's motion direction and the task endpoint. The higher the fit, the higher the contribution of this parameter to the score, ensuring that the robot's motion always faces the task endpoint and avoiding ineffective obstacle avoidance actions. , , , These are adjustable weighting coefficients for various evaluation indicators, used to adjust the priority of each indicator according to different operational scenarios, adapting to the operational needs of different scenarios. For example, in industrial warehousing scenarios, they can improve... The weights are adjusted to ensure the robot strictly follows the global path; in dynamic scenarios, this can improve... The weighting of these factors prioritizes obstacle avoidance safety. The EB-RRT emergency replanning submodule is used to deal with extreme and abnormal scenarios. When the global reference path is completely blocked by obstacles, or when there is no feasible speed combination and effective trajectory in the local planning, this submodule is triggered. It quickly generates a local path around the obstacle using the EB-RRT algorithm, guiding the robot to complete the obstacle avoidance. After the obstacle avoidance is completed, the robot is automatically controlled to return to the original global reference path, ensuring the continuity of the task and avoiding task interruption due to path blockage.

[0028] S5 specifically includes the following steps: Through the pose fusion calculation submodule, the wheel odometry and IMU data are fused to calculate the robot's real-time pose; through the MPC model predictive control submodule, the desired trajectory output by local planning is used as input, combined with the robot's kinematic model, to solve the optimal control quantity for trajectory tracking; the optimal control quantity is sent to the motor drive actuator to control the robot to complete the corresponding movement; through the motion state feedback submodule, the actual operation data of the robot is collected in real time and fed back to the positioning, planning and control links to complete closed-loop control. Specifically, the pose fusion and calculation submodule is used to fuse real-time data collected by the wheel odometry and IMU. Through multi-sensor data fusion algorithm, it calculates the robot's real-time pose in the global map, including position coordinates and orientation angle, to provide accurate real-time robot status data for trajectory tracking control, eliminate measurement errors of single sensors, and improve the accuracy and stability of pose calculation. The MPC model predictive control submodule is the core control unit in this stage. It takes the desired trajectory output from the local planning stage as input, combines the robot's kinematic model, and constructs the robot state prediction equation in the prediction time domain. At the same time, with the optimization objectives of minimizing trajectory tracking deviation and smoothing control quantity changes, it solves the optimal control quantity for rolling optimization, i.e., the robot's linear velocity and angular velocity control commands, in combination with the robot's hardware limit constraints. This submodule can achieve high-precision tracking of the desired trajectory, and adapt to the curvature changes of the trajectory in advance, improving the accuracy and smoothness of tracking control. The motor-driven actuator is used to receive the optimal control quantity issued by the MPC model predictive control submodule, convert the electrical signal into mechanical action, drive the robot's walking motor to execute the corresponding linear velocity and angular velocity, and complete the actual execution of the planned trajectory. It is the core actuator of robot motion control. The motion state feedback submodule is used to collect the robot's actual operating data in real time through wheeled odometry and IMU, including actual displacement, speed and attitude information. The collected actual operating data is fed back to the positioning, planning and control links in real time to form a closed-loop control of the whole process. It corrects the deviation between the robot's actual movement and the planned instructions in real time to ensure the accurate implementation of the planning results.

[0029] S6 specifically includes the following steps: Through a multi-level safety monitoring submodule, the minimum distance between the robot and obstacles, motion status, sensor status, and positioning accuracy are monitored throughout the process. Two-level safety thresholds are set: a first-level warning and a second-level braking. When the first-level warning threshold is triggered, deceleration and obstacle avoidance strategy adjustments are executed. When the second-level braking threshold is triggered, emergency stop is executed. Through an abnormal state identification submodule, abnormal states such as abnormal sensor data, continuous lack of feasible planned paths, and path deviation exceeding the threshold are identified, and corresponding degradation processing strategies are triggered. Through an emergency braking execution device, the robot completes emergency stop when the second-level braking threshold is triggered. Through a task closed-loop verification submodule, pose verification is completed after the robot reaches the target point to confirm task completion and wait for the next task instruction. Specifically, the multi-level safety monitoring submodule is used to monitor the robot's operating status in real time throughout the entire process, including the minimum distance between the robot and obstacles, real-time motion status, working status of each sensor, and global positioning accuracy. It also sets two levels of safety thresholds: a first-level warning threshold and a second-level braking threshold. When the first-level warning threshold is triggered, the robot is controlled to perform a deceleration action, and the local obstacle avoidance strategy is adjusted to avoid collision risks in advance. When the second-level braking threshold is triggered, an emergency stop action is immediately triggered to prevent collision accidents from occurring, providing graded safety monitoring and protection for the robot's entire operation. The abnormal state recognition submodule is used to identify various abnormal states during robot operation in real time, including abnormal or failed sensor data, no feasible planned path for multiple consecutive control cycles, and robot deviation from the global reference path exceeding a preset threshold. For different abnormal states identified, corresponding degradation processing strategies are triggered. For example, when a sensor fails, the robot is stopped in place and the fault is reported. When the path deviation exceeds the threshold, path regression planning is triggered to ensure the safe operation of the robot in abnormal scenarios. The emergency braking actuator is used to execute the robot's emergency stop action when the robot triggers the secondary braking threshold or when an emergency abnormal state is detected. It is the core actuator for robot safety backup and can quickly brake the robot in extreme scenarios to avoid collision accidents. The task closed-loop verification submodule is used to verify the robot's pose when the robot travels to the vicinity of the task target point, confirm that the deviation between the robot's actual pose and the target point pose is within the preset error range, confirm the completion of this navigation task, and wait for the issuance of the next task instruction, thus realizing the closed-loop management of the entire navigation task.

[0030] The method of use and working principle of this invention are as follows: Usage: First, multi-source sensors collect global environmental data of the work scene and motion data of the robot itself to complete map construction, map preprocessing, and kinematic calibration of the robot, clarifying the basic constraint boundaries of path planning. Then, it receives and parses task input commands, extracts the task's start and end points and corresponding global constraints, completes global path search and smoothing, generates a global reference path that conforms to the robot's motion constraints, and issues it. During the robot's operation along the global reference path, real-time environmental data from multiple sensors is collected simultaneously to complete data preprocessing, obstacle classification and identification, and motion trajectory prediction of dynamic obstacles, updating the local environmental map. The local planning window is constructed with the global reference path as the core, completing the screening of feasible speed combinations and the evaluation and selection of the optimal trajectory, generating corresponding obstacle avoidance control commands. When the path is blocked, emergency replanning is triggered simultaneously. The underlying actuator completes accurate tracking and closed-loop execution of the planned trajectory, monitors the robot's operating status and safe distance throughout the process, identifies various operating anomalies and triggers corresponding processing strategies, and finally completes pose verification and task closure upon reaching the task target point.

[0031] Working Principle: Employing a core architecture that integrates global path planning and local dynamic obstacle avoidance in a layered manner, the system first constructs a digital grid map of the operational scenario through multi-source sensor data acquisition and processing. Simultaneously, it performs kinematic calibration of the robot itself, providing fundamental constraints that align with the actual operational scenario and the robot's hardware capabilities for subsequent path planning. By introducing a cost function based on multi-objective constraints, the global path search algorithm is optimized, generating a global reference path that satisfies both task requirements and the robot's motion capabilities. This provides stable baseline guidance for the robot's entire navigation process, avoiding inflection points and obstacle-riding issues that violate motion constraints. During robot operation, multi-sensor synchronous data acquisition and fusion enable the operation cycle to be synchronized. The system enables real-time perception of the environment, classifies static and dynamic obstacles, and predicts the trajectory of dynamic obstacles in advance, achieving predictive obstacle avoidance rather than passive emergency response. At the same time, it constructs a multi-dimensional trajectory evaluation system with the global reference path as the core constraint to ensure that the robot always stays in line with the global task route during local obstacle avoidance and does not deviate from the task objective. High-precision tracking of the planned trajectory is achieved through model predictive control, and a closed-loop link of perception, planning and control is formed by combining feedback on the robot's real-time operating status. At the same time, hierarchical safety monitoring and full-scene anomaly handling mechanisms provide a safety net for the robot's entire operation, ultimately realizing autonomous path planning and safe obstacle avoidance control of the robot throughout the entire process.

[0032] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any way. Those skilled in the art can readily implement the present invention based on the description and drawings above. However, any modifications, alterations, and variations made by those skilled in the art without departing from the scope of the present invention using the disclosed technical content are equivalent embodiments of the present invention. Furthermore, any modifications, alterations, and variations made to the above embodiments based on the essential technology of the present invention are still within the protection scope of the present invention.

Claims

1. A method for autonomous path planning and obstacle avoidance control of a robot, characterized in that, Includes the following steps: S1. In the environmental modeling and kinematic calibration stage, the whole-domain data of the operation scene and the motion data of the robot body are collected by multi-source sensors to construct a 2D grid map of the operation scene, complete the map preprocessing and calibration of the core kinematic parameters of the robot, and determine the basic constraints of path planning. S2, Global Optimal Path Pre-planning Stage: Analyze the starting point, ending point, and global constraints of the task input, complete the global path search and path smoothing, generate a global reference path that conforms to the robot's motion constraints, and distribute it. S3, Real-time environmental perception and obstacle information processing stage: Simultaneously collect real-time data from multiple sensors, complete data denoising preprocessing, realize obstacle classification and dynamic obstacle motion trajectory prediction, and update the local rolling grid map. S4, Local Dynamic Obstacle Avoidance and Path Replanning Stage: Guided by the global reference path, a local rolling window is constructed to complete the selection of feasible speed windows and the evaluation and selection of the optimal trajectory, generating local obstacle avoidance control commands, and triggering emergency replanning when the path is blocked. S5, Trajectory Tracking and Low-Level Motion Control Stage: In the stage of trajectory tracking and low-level motion control, the robot's real-time pose is calculated, the optimal control quantity for trajectory tracking is solved, control commands are sent to the motor drive actuator, and the actual operation data of the robot is collected to complete the closed-loop feedback. S6. Safety backup and anomaly handling closed-loop stage: Monitor the robot's operating status and safe distance throughout the process, identify abnormal operating states and trigger corresponding handling strategies, and complete target point pose verification and task closure.

2. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S1 specifically includes the following steps: Simultaneously acquiring global point cloud data of the work scene and robot displacement, velocity, and attitude data through the lidar acquisition device, the wheeled odometer device, the IMU inertial measurement unit, and the ultrasonic blind spot acquisition device; generating a 2D grid map of the work scene through the global map construction submodule; and performing map preprocessing through the map preprocessing submodule. The block completes obstacle expansion processing of the grid map, completes the division of the absolutely infeasible region, restricted feasible region, and free feasible region, and completes the calibration of the robot's linear velocity, angular velocity, minimum turning radius, and maximum acceleration / deceleration threshold through the kinematic calibration submodule.

3. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S2 specifically includes the following steps: Extracting the global constraints of the task input—starting point, ending point, prohibited routes, priority channels, maximum path length, and task time limit—through the task instruction parsing submodule; and completing the global path search through the improved A* algorithm global path planning submodule. The path search employs a multi-objective cost function, the expression of which is: F = G + H + C Where F is the total cost of path search, G is the actual path length cost from the starting point to the current node, H is the estimated Manhattan distance cost from the current node to the target point, and C is the additional constraint cost, which includes the cost of turning times, channel priority, and safety distance. The inflection point fitting of the global path is completed through the cubic B-spline path smoothing submodule, and the path curvature continuity processing is completed to generate a global reference path that conforms to the robot motion constraints. The generated global reference path is then sent to the local planning stage.

4. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S3 specifically includes the following steps: The multi-sensor synchronous acquisition submodule completes the time synchronization and spatial calibration of multi-source data from the lidar, the ultrasonic blind spot acquisition device, the wheeled odometer, and the IMU; the point cloud data denoising submodule completes the pass-through filtering and statistical filtering of the lidar point cloud data, removing abnormal data and environmental noise; the obstacle classification and state update submodule completes the classification of static and dynamic obstacles, real-time updates of static obstacles on the local scrolling grid map, fitting of the position, velocity, and direction of motion of dynamic obstacles, and prediction of the future trajectory and occupied area of ​​dynamic obstacles.

5. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S4 specifically includes the following steps: constructing a sub-module through the local scrolling window. A local scrolling window is constructed centered on the robot's current pose. Through the feasible velocity window filtering submodule, feasible windows for linear and angular velocities are generated based on the robot's kinematic constraints and current motion state. Invalid velocity combinations that could lead to collisions or exceed hardware thresholds are eliminated. The trajectory evaluation and optimal instruction selection are completed through the globally guided DWA local planning submodule. The trajectory evaluation uses a multi-dimensional evaluation function, the expression of which is: Where Eval is the overall trajectory evaluation score, and path_dev is the distance between the simulated trajectory and the global reference path. The deviation value is defined as follows: obs_dist is the minimum distance between the simulated trajectory and the obstacle; vel_smooth is the smoothness parameter of the velocity change; goal_dir is the degree of fit of the simulated trajectory toward the target point; α, β, γ, and δ are the adjustable weight coefficients of each evaluation index. The velocity combination with the highest score is selected as the control command for the current cycle. When the global reference path is completely blocked or the local planning has no feasible solution, the EB-RRT* emergency replanning submodule generates a local obstacle avoidance path. After the obstacle avoidance is completed, the robot is controlled to return to the global reference path.

6. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S5 specifically includes the following steps: The pose fusion calculation submodule fuses the data from the wheel odometer and the IMU to calculate the robot's real-time pose; the MPC model predictive control submodule uses the desired trajectory output from local planning as input, combined with the robot's kinematic model, to solve for the optimal control quantity for trajectory tracking; the optimal control quantity is then sent to the motor drive actuator to control the robot to complete the corresponding movement; and the motion state feedback submodule collects the robot's actual operating data in real time and feeds it back to the positioning, planning, and control stages to complete closed-loop control.

7. The robot autonomous path planning and obstacle avoidance control method according to claim 1, characterized in that, S6 specifically includes the following steps: Through the multi-level safety monitoring submodule, the minimum distance between the robot and obstacles, its motion state, sensor status, and positioning accuracy are monitored throughout the process. Two-level safety thresholds are set: a first-level warning threshold and a second-level braking threshold. When the first-level warning threshold is triggered, deceleration and obstacle avoidance strategy adjustments are executed; when the second-level braking threshold is triggered, emergency braking is executed. Through the abnormal state identification submodule, abnormal states such as abnormal sensor data, continuous lack of feasible planned paths, and path deviation exceeding thresholds are identified. The corresponding degradation processing strategy is triggered. The emergency braking execution device completes the robot's emergency stop when the secondary braking threshold is triggered. The task closed-loop verification submodule completes the pose verification after the robot reaches the target point, confirms the task completion, and waits for the next task instruction.

Images