Robot path control method based on automatic operation

Abstract

The invention relates to the technical field of robot path control, and discloses a robot path control method based on automatic operation. The method comprises the following steps: acquiring three-dimensional point cloud data of a robot working environment, and generating an environment grid map through spatial rasterization processing; a dynamic obstacle track is extracted based on the map, and a dynamic obstacle avoidance constraint condition is constructed in combination with kinematics parameters of the robot. After the path search space is initialized according to the constraint condition, a candidate path set is generated by adopting a heuristic strategy, and an optimized path meeting a preset threshold value is screened out through smoothness and energy consumption evaluation. And inputting the optimized path into a track generator to generate a continuous motion track of each joint of the robot. And pose deviation is monitored in real time during operation, and track re-planning is triggered if the deviation exceeds a fault-tolerant threshold value. According to the method, the environment sensing precision and the path generation efficiency are improved, and the stable operation of the robot in a complex dynamic environment is ensured.

Description

Technical Field

[0001] This invention relates to the field of robot path control technology, specifically to a robot path control method based on automated operation. Background Technology

[0002] In fields such as industrial automation, logistics warehousing, and service robots, the application of automated robot operation technology is becoming increasingly widespread. Path control, as the core component of automated robot operation, directly determines operational efficiency, safety, and stability. With the increasing complexity of the working environment, the frequent appearance of dynamic obstacles, and the real-time changes in the working scenario, higher demands are placed on robot path control technology. Traditional robot path control methods are mostly based on static environment models, generating fixed paths by pre-collecting environmental information. While these methods can generally meet the needs in static, interference-free working scenarios, they often exhibit significant limitations in complex environments with dynamic interference such as personnel movement and equipment movement.

[0003] In existing technologies, some path control methods attempt to introduce dynamic obstacle avoidance mechanisms, but most only obtain environmental information through two-dimensional images or simple distance sensors. This makes it difficult to comprehensively and accurately reflect the three-dimensional structure of the workspace, resulting in insufficient accuracy in obstacle positioning and a tendency for untimely or misjudged obstacle avoidance. For example, in warehouse robot operation scenarios, environmental maps built solely from two-dimensional LiDAR data cannot distinguish the height differences of stacked goods. When a tall obstacle temporarily appears, the robot may cause a collision because it cannot perceive the height information. Furthermore, traditional methods often overemphasize the shortest path distance during path generation, neglecting the robot's kinematic characteristics. The generated path may contain many sections with sharp turns and abrupt angle changes, leading to discontinuous joint movements during robot execution. This not only increases energy consumption but may also shorten equipment lifespan due to mechanical impact.

[0004] Existing path optimization strategies lack a comprehensive evaluation system, with most using only path length as the core evaluation indicator. They fail to adequately consider key factors such as path smoothness and energy consumption, making it difficult to balance operational efficiency and cost in actual execution. During path execution, traditional methods are slow to monitor and respond to robot pose deviations. When pose shifts occur due to uneven ground, load changes, or other factors, the replanning mechanism cannot be triggered in time, affecting operational accuracy and even causing task failure. For example, in industrial assembly robot operations, if a small pose deviation occurs during robot movement and is not corrected in time, it may lead to misaligned parts and product quality issues. With the continuous expansion of robot application scenarios, the requirements for real-time performance, reliability, and economy in path control are increasing. Existing technologies are insufficient to meet the needs of automated operations in complex dynamic environments, necessitating a robot path control method that can accurately perceive the environment, scientifically generate paths, comprehensively optimize and evaluate, and possess real-time error correction capabilities. Summary of the Invention

[0005] The purpose of this invention is to provide a robot path control method based on automated operation to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides a robot path control method based on automated operation, the method comprising: Acquire 3D point cloud data of the robot's current working environment, perform spatial rasterization processing on the 3D point cloud data, and generate an environmental raster map; Dynamic obstacle trajectories are extracted from environmental grid maps, and dynamic obstacle avoidance constraints are constructed by combining robot kinematic parameters. The path search space is initialized based on the dynamic obstacle avoidance constraints, and a set of candidate paths is generated in the path search space using a heuristic strategy. For each path in the candidate path set, perform smoothness and energy consumption evaluations, and select the optimized path that meets the preset evaluation threshold. The optimized path is input into the trajectory generator to generate the continuous motion trajectory of each joint of the robot; The robot monitors its pose deviation in real time when executing its motion trajectory. If the pose deviation exceeds the fault tolerance threshold, trajectory replanning is triggered.

[0007] Preferably, the spatial rasterization processing of the 3D point cloud data includes: Set the raster resolution parameters and divide the 3D point cloud data into cubic units of equal volume according to the resolution parameters; Calculate the point cloud density within each cubic cell, and mark cells with a density below a first threshold as free space and cells with a density above a second threshold as obstacle space; Morphological closing operations are performed on adjacent obstacle spatial units to generate connected region labels.

[0008] Preferably, the dynamic obstacle avoidance constraints include: Extract the continuously changing sequence of obstacle spatial units in the connected region labels and calculate the displacement vector of the center point of the sequence; Predict the area occupied by dynamic obstacles within a future time window based on displacement vectors; By combining the robot's body radius and maximum deceleration, the minimum safe distance between the area occupied by dynamic obstacles and the robot's path is calculated.

[0009] Preferably, the generated candidate path set includes: Select the line connecting the robot's current position and the target position in the environmental grid map as the reference direction; A detection ray is emitted at a preset angle interval along the reference direction. When the detection ray encounters an obstacle space, a path turning point is generated. Match the path turning points with the maximum curvature constraints in the robot's kinematic model, and eliminate combinations of turning points that do not meet the curvature constraints.

[0010] Preferably, the smoothness evaluation includes: calculating the rate of change of steering angle between adjacent path segments in the candidate path, counting the number of path segments whose rate of change of steering angle exceeds a critical value, and using the proportion of the number as the smoothness score; The energy consumption assessment includes: calculating the joint torque integral required for each path segment based on the robot mass distribution model, and using the ratio of the torque integral to the path length as the energy consumption score.

[0011] Preferably, the process of selecting the optimized path that meets the preset evaluation threshold includes: The smoothness score and energy consumption score are weighted and fused to generate a comprehensive path score; The candidate path with the best comprehensive path score is selected as the optimized path; If the overall path score of all candidate paths fails to reach the preset evaluation threshold, the smoothness score weight will be reduced and the path will be re-selected.

[0012] Preferably, the generation of continuous motion trajectories for each joint of the robot includes: The optimized path is discretized into a sequence of path points with equal time intervals; Perform cubic spline interpolation on the path point sequence to generate the position-time function in the joint space; Based on the first and second derivatives of the position-time function, joint velocity curves and acceleration curves are generated respectively.

[0013] Preferably, the pose deviation of the robot during real-time monitoring of its motion trajectory includes: The Euclidean distance between the actual position and the theoretical position is obtained through the inertial measurement unit of the robot's end effector; The absolute error between the actual angle and the theoretical angle of each joint is obtained by using a joint encoder; When the Euclidean distance or the absolute error of any joint exceeds the tolerance threshold, it is determined that the pose deviation exceeds the limit.

[0014] Preferably, the trigger trajectory replanning includes: Freeze the robot's current motion state and obtain the latest environmental grid map; Reinitialize the path search space starting from the robot's current position; The optimized path is regenerated using a shortened prediction time window.

[0015] Preferably, the regeneration of the optimized path includes: In the path search space, prioritize retaining the unexecuted segments of the original optimized path; The executed segments of the original optimized path are dynamically checked for obstacle avoidance constraints. If the check fails, a greedy algorithm is used to generate a local detour path. The local detour path is smoothly spliced ​​with the unexecuted segment and then output to the trajectory generator.

[0016] Compared with the prior art, the beneficial effects of the present invention are: In the environmental perception stage, a grid map of the environment is generated by acquiring 3D point cloud data and performing spatial rasterization processing. Compared with traditional 2D environmental modeling methods, this approach can capture the three-dimensional information of the work space more comprehensively and accurately. Whether it is the position, shape, or height characteristics of obstacles, they can all be clearly presented in the grid map. This 3D environmental modeling method effectively avoids the simplification and loss of environmental information in 2D data, making the robot's perception of the work environment closer to the actual situation. This lays a solid environmental data foundation for subsequent path planning, especially in complex scenarios with obstacles of varying heights. It can completely identify potential obstacles from a spatial dimension, reducing the risk of path planning errors caused by incomplete environmental perception.

[0017] In the dynamic obstacle avoidance and path generation stages, by extracting the trajectories of dynamic obstacles and constructing constraints based on the robot's kinematic parameters, a deep integration of obstacle avoidance strategies and the robot's own motion characteristics is achieved. Trajectory extraction allows the robot to understand the motion patterns of obstacles and predict their trends in advance. The constraints constructed using the robot's kinematic parameters ensure that the path planning scheme conforms to the robot's mechanical motion capabilities, avoiding the generation of paths exceeding the robot's joint range of motion, speed limits, or acceleration tolerance. Based on this, a heuristic strategy is used to initialize the path search space and generate a set of candidate paths. This significantly improves the efficiency of path search while ensuring path feasibility, avoiding the waste of computational resources and path generation delays caused by traditional blind search methods. This enables the robot to respond quickly to dynamic environmental changes and promptly plan path schemes that adapt to these changes.

[0018] The design of the path evaluation and optimization process breaks through the limitations of traditional methods that rely on a single evaluation index. It comprehensively screens candidate paths through smoothness and energy consumption assessments. Smoothness assessment effectively eliminates paths with sharp turns or abrupt changes, ensuring that the optimized path meets the continuity requirements of robot joint movements, reducing mechanical shocks during robot movement, lowering the risk of equipment failure, and improving operational stability, thus preventing a decrease in operational accuracy due to path bumps. Energy consumption assessment, from an operating cost perspective, selects path solutions with lower energy consumption. For robots requiring long-term continuous operation, this effectively reduces energy consumption, lowers operating costs, and improves equipment endurance. This multi-dimensional evaluation and screening mechanism allows the optimized path to balance operational efficiency, equipment protection, and economic costs, enhancing the overall practicality of the path solution.

[0019] In the path execution and monitoring phase, the optimized path is transformed into continuous motion trajectories for each joint, ensuring the robot can smoothly and accurately execute path planning instructions, reducing jerky joint movements, and further improving operational accuracy. The mechanism of real-time monitoring of pose deviations and triggering replanning provides double assurance for the reliability of path execution. When the robot experiences pose deviations due to external interference, mechanical wear, or other factors, it can be detected promptly, initiating the replanning process and preventing operational failures or safety accidents caused by accumulated deviations. This closed-loop control mode enables the robot to continuously adapt to environmental and state changes throughout the entire operation, improving the robustness of path control and ensuring the smooth completion of tasks in complex and ever-changing environments. Whether in industrial production, logistics handling, or service operations, it demonstrates stable and reliable operational performance, enhancing the practicality and competitiveness of the robotic automated operation system. Attached Figure Description

[0020] Figure 1This is a schematic diagram illustrating the working principle of the robot path control method based on automated operation described in this invention. Figure 2 A flowchart for constructing dynamic obstacle avoidance constraints; Figure 3 A flowchart for generating a set of candidate paths; Figure 4 This is a comprehensive evaluation and analysis diagram of the path; Figure 5 This is a diagram showing the results of pose deviation monitoring. Detailed Implementation

[0021] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0022] Please see Figure 1 This invention provides a robot path control method based on automated operation. The method includes: first, acquiring 3D point cloud data of the robot's current working environment, where the 3D point cloud data is collected by a LiDAR or depth camera mounted on the robot, providing geometric information of the environment; second, performing spatial rasterization processing on the 3D point cloud data, converting the irregular point cloud into a regular structured map, generating an environment raster map; third, dividing the point cloud into uniform cells and classifying spatial types based on point cloud density; fourth, extracting dynamic obstacle trajectories based on the environment raster map, obtained by analyzing obstacle position changes in continuous time frames; fifth, constructing dynamic obstacle avoidance constraints by combining robot kinematic parameters, such as maximum speed and steering angle, defining safe interaction rules between the robot and obstacles; and sixth, initializing a path search space according to the dynamic obstacle avoidance constraints, which is a discrete representation of the environment, in which a heuristic strategy is used to generate a set of candidate paths, with the heuristic strategy using a cost function to guide the search direction. For each path in the candidate path set, smoothness and energy consumption are evaluated. Smoothness evaluation quantifies the curvature of the path, and energy consumption evaluation estimates the motion energy consumption. Optimized paths that meet preset evaluation thresholds are selected, which are set based on application requirements. The optimized paths are input into a trajectory generator, which converts the paths into time-parameterized motion commands, generating continuous motion trajectories for each joint of the robot. The robot's pose deviation during motion trajectory execution is monitored in real time. The pose deviation is obtained by comparing sensor data with theoretical values. If the pose deviation exceeds a tolerance threshold, trajectory replanning is triggered. The tolerance threshold is adjusted according to the robot's accuracy requirements.

[0023] Example 1: See Figure 2 The grid resolution parameter is a key parameter determining the accuracy of the environmental grid map, and it needs to be set according to the robot's operating scale. For mobile robots operating in large warehouse environments, the grid resolution parameter can be set to 0.1 meters, while for robotic arms performing precision assembly, it may need to be set to 0.01 meters or smaller. Based on the resolution parameter, the 3D point cloud data is divided into cubic units of equal volume. The side length of each cubic unit is strictly equal to the grid resolution parameter, and each 3D point is assigned to a cubic unit according to its spatial coordinates. Calculating the point cloud density within each cubic unit is the basis for distinguishing between passable and obstacle areas. The point cloud density is obtained by calculating the total number of 3D points contained within each cubic unit. Units with a density below a first threshold are marked as free space. The first threshold is a small integer value used to identify blank areas with almost no point cloud; for example, the first threshold could be set to 5 points. Units with a density above a second threshold are marked as obstacle space. The second threshold is a large integer value used to identify solid areas where point clouds are clustered; for example, the second threshold could be set to 50 points. Cube cells with densities between a first threshold and a second threshold can be marked as unknown or undetermined regions. In some embodiments, the calculation of point cloud density may take into account the intensity or color information of the point cloud, assigning different weights to different points.

[0024] In practice, morphological closing operations on adjacent obstacle spatial units are essential for generating a coherent obstacle map. These operations involve first dilation and then erosion. The dilation operation uses a predefined structuring element, such as a 3x3x3 cube kernel, to traverse all cube units marked as obstacle spaces, temporarily marking all adjacent units within the structuring element's coverage area as obstacles. The erosion operation uses the same structuring element to traverse all temporarily dilated obstacle units; only when all units within the structuring element's coverage area belong to the obstacle space is the central unit retained as the final obstacle space unit. Connected region labels are generated using a region growing algorithm. Starting from any obstacle spatial unit, the algorithm recursively checks all its neighboring units, assigning the same unique label identifier to interconnected obstacle units. Morphological closing operations effectively fill the voids on the obstacle surface caused by sparse point clouds and bridge narrow gaps, thus preventing the creation of incorrect narrow passages during path planning. It is understandable that connected component labels enable the system to distinguish different, independent obstacles in the environment, providing a basis for subsequent tracking of the movement trajectory of individual dynamic obstacles.

[0025] In practical implementation, extracting the continuously changing sequence of obstacle spatial units from connected component labels is crucial for achieving dynamic obstacle tracking. This process needs to be performed on a continuous multi-frame environmental grid map. For each obstacle with a unique connected component label, its corresponding set of obstacle spatial units is located in the continuous frame map of timestamps tn, t-n+1, ​​..., t. Calculating the displacement vector of the sequence center point requires first calculating the average of the three-dimensional coordinates of all spatial units of the obstacle in each frame to obtain the coordinates of the center point of that frame. The displacement vector is then directly calculated from the vector difference of the center point coordinates of two adjacent frames, for example, the displacement vector of the center point from timestamp t-1 to timestamp t. Predicting the occupied area of ​​a dynamic obstacle within a future time window based on the displacement vector typically employs a linear motion model, assuming the obstacle maintains its current speed and direction in the short term. The length of the future time window needs to be reasonably set based on the robot's control cycle and the maximum speed of the dynamic obstacle, for example, set to 2 seconds in the future. The prediction process involves using the center point of the current frame as the starting point, dividing the displacement vector by the time interval to obtain the velocity vector as the motion speed, and extrapolating the position of the obstacle's center point at each moment within the future time window. Based on the predicted center point location, and combined with the shape and size of the original spatial unit set of the obstacle, the spatial area that it may occupy at each future moment is determined. In some embodiments, for obstacles with complex motion patterns, state estimation algorithms such as Kalman filters can be used to improve the accuracy of displacement vector and future position prediction.

[0026] In practical implementation, calculating the minimum safe distance by combining the robot's body radius and maximum deceleration is a core constraint for ensuring dynamic obstacle avoidance safety. The robot's body radius is a fixed value obtained from the robot's mechanical parameters, defining the radius of the smallest circumscribed cylinder or sphere occupied by the robot. The robot's maximum deceleration is a physical limit determined by the braking capability of the robot's drive system; it represents the maximum deceleration the robot can achieve in an emergency. Calculating the minimum safe distance between the area occupied by the dynamic obstacle and the robot's path requires considering the worst-case scenario, i.e., the scenario where the robot and obstacle approach each other directly along the relative motion direction. The formula for calculating the minimum safe distance needs to include the following components: the robot's body radius, the radius of the obstacle's positional uncertainty within the prediction time window, and the braking distance calculated based on the robot's maximum deceleration. The braking distance is calculated based on the robot's current speed and maximum deceleration, representing the shortest distance required from the start of braking to complete stop. Optionally, the minimum safe distance can also include an additional safety margin to address uncertainties caused by sensor noise and control delays. Optionally, different safety margins can be used for dynamic obstacles with different movement speeds, with higher-speed obstacles requiring a larger safety margin. The calculated minimum safe distance will serve as a hard constraint. In subsequent path searches, any path point whose distance to the predicted area occupied by a dynamic obstacle is less than the minimum safe distance will be directly excluded. This physical model-based safe distance calculation can, in principle, prevent collisions between the robot and dynamic obstacles, providing a safety guarantee for the robot's autonomous operation in dynamic environments.

[0027] Example 2: See Figure 3In the environmental grid map, the line connecting the robot's current position and the target position is selected as the reference direction. The robot's current position is obtained through odometry or a global positioning system, while the target position is given by the task command. The reference direction is represented in 3D space as a vector pointing from the robot's current position to the target position, defining the globally dominant direction for path searching. Probe rays are emitted along the reference direction at preset angular intervals. The preset angular interval is a fixed value, such as 15 degrees. The emission method is a conical coverage emission with the robot's current position as the origin and the reference direction as the axis in 3D space. The number of probe rays emitted is determined by the preset angular interval; for example, an interval of 15 degrees will emit hundreds of rays in both the horizontal and vertical planes. The probe rays extend from the origin in the environmental grid map in the specified direction using a raycasting algorithm. When a ray encounters an obstacle space, a path turning point is generated, and the position of the path turning point is recorded as the coordinates of the intersection point between the ray and the boundary of the obstacle space grid. In practice, matching path turning points with the maximum curvature constraints in the robot's kinematic model is a screening process. These maximum curvature constraints stem from the robot's physical structure's steering limitations; for example, for a wheeled robot, the maximum curvature constraint corresponds to the minimum turning radius. The matching operation verifies whether the virtual arc segment connecting two adjacent path turning points satisfies the curvature constraint, i.e., calculating whether the radius of curvature of this arc segment is greater than or equal to the robot's minimum turning radius. Eliminating turning point combinations that do not meet the curvature constraints removes those turning points that would cause excessive path curvature. For example, if the radius of curvature of a path segment formed by three consecutive path turning points is less than the robot's minimum turning radius, the middle turning point will be eliminated. It can be understood that by rapidly generating a large number of potential path points through a ray-detection mechanism and then filtering them through kinematic constraints, this method can efficiently generate initial path options that both avoid obstacles and conform to the robot's physical movement capabilities. The generation and screening of path turning points is a crucial step connecting the environmental map with the robot's own movement capabilities, ensuring the feasibility of subsequent path planning.

[0028] In practical implementation, smoothness evaluation is a quantitative analysis of each complete path in the candidate path set. Calculating the rate of change of steering angle between adjacent path segments in the candidate path is the core operation of smoothness evaluation. An adjacent path segment refers to two consecutive line segments formed by three consecutive path turning points. The rate of change of steering angle is the rate of change of the angle between the vectors of the first and second line segments. The angle is calculated using the vector cross product and dot product formulas, and the rate of change is obtained by dividing this angle by the average length of the two line segments. The number of path segments with a rate of change of steering angle exceeding a critical value is counted. The critical value is a preset threshold for the rate of change of angle, such as 5 degrees per meter, set based on the robot's motion stability requirements. The proportion of these paths is used as the smoothness score. The proportion refers to the ratio of the number of path segments with a rate of change of steering angle exceeding the critical value to the total number of path segments in the candidate path set. The smoothness score is inversely proportional to the proportion; a higher proportion results in a lower smoothness score, indicating a less smooth path. In some embodiments, the calculation of the rate of change of steering angle can consider full attitude changes in three-dimensional space, not just directional changes on the horizontal plane. In some embodiments, the threshold value can be adaptively adjusted according to the local environmental density of the path segment, allowing for a larger rate of change of steering angle in areas with dense obstacles.

[0029] In practical implementation, energy consumption assessment is based on quantitative calculations using the robot's dynamics model. Calculating the joint torque integral required for each path segment according to the robot's mass distribution model is fundamental to energy consumption assessment. The robot's mass distribution model includes the mass, center of mass position, and inertia tensor parameters of each link in the robot; these parameters are inherent dynamic properties of the robot. For wheeled mobile robots, the joint torque integral mainly considers the torque of the drive wheels; for robotic arms, it is necessary to calculate the torque of all joints. Calculating the joint torque integral required for each path segment requires the use of a robot inverse dynamics algorithm. This algorithm calculates the instantaneous torque values ​​of each joint required to execute the movement of that path segment based on the expected displacement, velocity, acceleration, gravity, friction, and other factors along the path segment. The joint torque integral is the sum of squares of the instantaneous torques of each joint over the entire path segment's movement time. This sum of squares reflects the energy consumption during the movement. The ratio of the torque integral to the path length is used as the energy consumption score. The path length is the spatial curve length of the path segment, and the energy consumption score is an estimate of the energy consumed per unit path length. A lower energy consumption score indicates higher energy efficiency for that path segment. Optionally, when calculating the joint torque integral, nonlinear factors such as friction can be ignored, and a simplified dynamic model can be used to improve calculation speed. Optionally, the energy consumption score can integrate the torque contributions of different joints, assigning different weighting coefficients based on the energy efficiency characteristics of the motors at each joint. It can be understood that smoothness assessment and energy consumption assessment characterize the quality of the path from different dimensions. Smoothness assessment focuses on the continuity and comfort of the movement, while energy consumption assessment focuses on the economy and endurance of the movement. The combination of the two provides a comprehensive basis for selecting high-quality paths.

[0030] Example 3: In specific implementation, the weighted fusion of smoothness score and energy consumption score is a key step in generating a comprehensive path score. The smoothness score is derived from the statistical results of the proportion of path segments whose steering angle change rate exceeds a critical value. The energy consumption score is calculated based on the ratio of joint torque integral to path length. Since the smoothness score is a dimensionless ratio while the energy consumption score has physical dimensions, direct weighting would lead to dimensional inconsistency. Therefore, normalization is required to convert both into dimensionless values. The normalization process uses a minimum-maximum scaling method. For the smoothness score, the minimum and maximum values ​​of the smoothness scores for all candidate paths are calculated, and the smoothness score of each candidate path is converted into a proportional value relative to the minimum and maximum values. Similarly, for the energy consumption score, the minimum and maximum values ​​of the energy consumption scores for all candidate paths are calculated, and the energy consumption score of each candidate path is converted into a proportional value relative to the minimum and maximum values. After normalization, both the smoothness score and the energy consumption score are scaled to dimensionless values ​​between zero and one, thus ensuring dimensional consistency in the weighted fusion. The weighted fusion process requires pre-setting weight coefficients for smoothness and energy consumption scores. The initial values ​​of these weight coefficients are determined based on the robot's specific task requirements. For example, in precision machining tasks requiring smooth motion, a higher weight coefficient is set for the smoothness score, while in long-distance movement tasks emphasizing energy efficiency, a higher weight coefficient is set for the energy consumption score. The mathematical expression for generating the comprehensive path score is: multiply the normalized smoothness score by its weight coefficient, add the normalized energy consumption score multiplied by its weight coefficient, and the sum is the comprehensive path score. The formula for calculating the comprehensive path score is as follows:

[0031] Where: the character C represents the overall path score. The character represents the weighting coefficient of the smoothness score, and the character S represents the normalized smoothness score value. The weighting coefficient represents the energy consumption score, and the character E represents the normalized energy consumption score value.

[0032] The optimal candidate path is selected by iterating through the comprehensive path scores of all paths in the candidate path set. The comparison process uses a sorting algorithm to find the minimum or maximum comprehensive path score, depending on the design direction of the scoring system. If the comprehensive path score represents cost, the path with the minimum score is selected; if it represents benefit, the path with the maximum score is selected. If none of the candidate paths' comprehensive path scores reach the preset evaluation threshold (a predefined acceptable standard), a weight adjustment strategy is implemented. This reduces the weight of the smoothness score and re-selects the path. Reducing the smoothness score weight is achieved by decreasing its weight coefficient, while simultaneously increasing the energy consumption score's weight coefficient to maintain overall weight balance. The re-selection process involves recalculating the comprehensive path scores of all candidate paths using the adjusted weight coefficients and checking again if any path's comprehensive path score reaches the preset evaluation threshold. This iterative process can be repeated until a suitable path is found or the maximum number of iterations is reached.

[0033] In practice, discretizing the optimized path into a sequence of path points with equal time intervals is a preprocessing operation before trajectory generation. The optimized path is a continuous curve composed of a series of three-dimensional spatial coordinate points. The discretization operation first requires determining the total motion time and sampling time interval of the path. The total motion time is estimated by the ratio of the total length of the optimized path to the robot's expected average speed. The sampling time interval is determined by the robot's control cycle; for example, if the control cycle is 20 milliseconds, the time interval is set to 0.02 seconds. The sequence of path points with equal time intervals is generated by sampling position points along the optimized path curve at equal time intervals. Each path point contains a timestamp and its corresponding three-dimensional coordinate value. Cubic spline interpolation is performed on the path point sequence to obtain a smooth joint motion trajectory. Cubic spline interpolation constructs a cubic polynomial function between adjacent path points, ensuring that the entire position-time curve has continuous first and second derivatives at the path points. Generating the position-time function in joint space requires mapping the path points in Cartesian space to the angle values ​​of each joint through robot inverse kinematics calculations. Inverse kinematics calculations solve for the joint angles based on the robot's configuration parameters. The position-time function is a mathematical function with time as the independent variable and joint angle as the dependent variable, describing how each joint angle changes over time. In some embodiments, the discretization process can consider the robot's dynamic constraints, employ adaptive time interval sampling, and densify sampling points in areas with high path curvature to improve trajectory accuracy. In some embodiments, cubic spline interpolation can apply additional boundary conditions, such as setting the velocities at the start and end points to zero, to generate trajectories that more closely resemble actual motion states.

[0034] Generating joint velocity and acceleration curves based on the first and second derivatives of the position-time function is the core step in trajectory generation. The first derivative represents the rate of change of joint angle with time, i.e., joint velocity, and the second derivative represents the rate of change of joint velocity with time, i.e., joint acceleration. Derivative calculations can be performed analytically by differentiating the position-time function. For a piecewise cubic spline function, the first and second derivatives of each segment can be directly calculated using polynomial differentiation rules. The joint velocity curve is formed by connecting the first derivative values ​​at each time point, defining the velocity profile of each joint. The joint acceleration curve is formed by connecting the second derivative values ​​at each time point, defining the acceleration profile of each joint. These curves need to be checked to ensure they meet the physical limits of the robot joints, such as maximum velocity limits, maximum acceleration limits, and maximum jerk limits. If any limits are exceeded, the trajectory needs to be scaled or replanned. It is understandable that the position-time function and its derivative curve generated by cubic spline interpolation can ensure the continuity and smoothness of joint motion, avoiding rigid impacts and vibrations during movement. This plays a crucial role in improving the robot's operational accuracy and extending its mechanical lifespan. It is also understandable that the process from generating the comprehensive path score to outputting the continuous motion trajectory is a process that closely integrates multi-objective optimization with trajectory planning techniques. This ensures that the robot's final trajectory is not only geometrically feasible but also achieves optimal motion performance.

[0035] In some embodiments, the weighted fusion process can employ more complex normalization methods, such as Z-score standardization, to convert the smoothness score and energy consumption score into dimensionless values ​​with a mean of zero and a standard deviation of one based on the mean and standard deviation of all candidate path scores, thereby better handling the distribution characteristics of the score data. In some embodiments, the preset evaluation threshold can be designed as a dynamic adjustment mechanism, calculating the threshold value in real time according to the density and distribution of obstacle space in the environmental grid map, thus adapting to operating environments of varying complexity. Optionally, in the comprehensive path score calculation, a nonlinear fusion function can be introduced, such as using a weighted geometric mean or a utility function-based fusion method, to better handle the nonlinear relationship between the smoothness score and the energy consumption score. In some embodiments, the trajectory generation stage can combine joint moment constraints to directly optimize the path point sequence, ensuring that the generated trajectory is not only smooth but also dynamically feasible.

[0036] See Figure 4This chart presents the comprehensive evaluation results of candidate paths in robot path planning. By normalizing the smoothness and energy consumption scores of multiple candidate paths and using a weighted fusion method to generate a comprehensive path score, the path with the optimal comprehensive score is ultimately selected as the execution path. The chart includes three key evaluation indicators: blue bars represent the normalized path smoothness score, reflecting the smoothness of turning; red bars show the normalized energy consumption score, reflecting the energy efficiency of path execution; and green bars display the weighted fusion comprehensive path score, comprehensively considering the balance between motion stability and energy consumption. The vertical red dashed line indicates the optimal path selection result under the current weight configuration. This multi-objective optimization method ensures that the robot can meet motion stability requirements while also considering energy efficiency, providing the most suitable path planning scheme for different operational scenarios.

[0037] Example 4: In practical implementation, real-time monitoring of the robot's pose deviation during motion trajectory execution is a continuous process. The Euclidean distance between the actual and theoretical positions is obtained through the inertial measurement unit (IMU) of the robot's end effector. The IMU typically includes a three-axis accelerometer and a three-axis gyroscope, capable of measuring the linear acceleration and angular velocity of the end effector in three-dimensional space. Calculating the actual position requires a double integration of the linear acceleration measured by the IMU. During integration, the influence of gravitational acceleration needs to be compensated for, and sensor drift needs to be addressed. The theoretical position comes from the coordinate values ​​of the position-time function output by the trajectory generator at the corresponding moment. The Euclidean distance is calculated as the square root of the sum of the squares of the differences between the actual and theoretical position coordinates in three-dimensional space. The Euclidean distance reflects the absolute position deviation of the end effector in space. The absolute error between the actual and theoretical angles of each joint is obtained through joint encoders. Joint encoders are installed on each joint motor of the robot, directly measuring the rotational displacement of the joints. The actual angles are the real-time readings of the joint encoders, while the theoretical angles come from the joint angle values ​​of the position-time function output by the trajectory generator at the corresponding moment. Absolute error is the absolute value of the difference between the actual angle and the theoretical angle, reflecting the individual angle tracking deviation of each joint. When the Euclidean distance or the absolute error of any joint exceeds the tolerance threshold, it is determined that the pose deviation exceeds the limit. The tolerance threshold is set according to the robot's accuracy requirements and task type. For example, the tolerance threshold is smaller for high-precision assembly tasks, while it is larger for material handling tasks. The monitoring system cyclically executes data acquisition, calculation, and comparison logic with a fixed control cycle to ensure timely detection of pose deviation exceeding the limit. Refer to Table 1 for the key parameters involved in the pose deviation monitoring process and their typical settings.

[0038] Table 1: Position Deviation Monitoring Parameters Monitoring parameters Data source Calculation method Typical fault tolerance threshold range Over-limit processing End position Euclidean distance Inertial Measurement Unit Square root of the sum of squares of the differences between the actual and theoretical three-dimensional coordinates 1.0 mm to 10.0 mm Trigger trajectory replanning absolute error of joint angle Joint encoder The absolute value of the difference between the actual angle and the theoretical angle 0.5 degrees to 2.0 degrees Trigger trajectory replanning Monitoring cycle System clock fixed time interval 10 milliseconds to 100 milliseconds Periodically execute monitoring logic In practical implementation, calculating the Euclidean distance requires coordinating the transformation relationships between different sensor coordinate systems. The inertial measurement unit's (IMU) measurements are expressed in the sensor coordinate system, while the theoretical position is defined in the world coordinate system or base coordinate system. Coordinate transformation requires converting the IMU data to a reference system consistent with the world coordinate system using a pre-calibrated transformation matrix, which is determined through a hand-eye calibration process. The calculation of the actual position also involves filtering the sensor data, using Kalman filters or complementary filters to fuse the accelerometer and gyroscope data to reduce the impact of noise and drift on position estimation. Obtaining the theoretical position requires a high-precision time synchronization mechanism to ensure that the theoretical position value read from the trajectory generator corresponds to the same moment as the IMU data. It can be understood that monitoring the Euclidean distance at the end position provides a global evaluation index of the robot's overall pose accuracy, while monitoring the absolute error of joint angles reveals the execution consistency of each joint from the drive level. The combination of the two provides a multi-dimensional basis for comprehensively evaluating trajectory tracking performance.

[0039] In practical implementation, data reading from the joint encoders is accomplished via real-time bus communication protocols, such as EtherCAT or CANopen, ensuring low-latency acquisition of the actual angle values ​​of each joint. The calculation of the actual angles needs to consider encoder resolution, converting encoder counts into angle values ​​while performing zero-position offset correction. Obtaining the theoretical angles requires real-time querying of the position-time function value from the trajectory generator at the current system time; this query process demands a deterministic real-time response from the trajectory generator. The calculated absolute error needs to be compared in real-time with a preset fault tolerance threshold. The comparison logic is implemented through a hardware comparator or real-time software loop to ensure timely triggering of subsequent processing flows in case of exceeding limits. When the Euclidean distance or the absolute error of any joint exceeds the fault tolerance threshold, the system generates a pose deviation exceeding limit event. This event interrupts the normal trajectory execution process and activates the trajectory replanning mechanism. In some embodiments, the fault tolerance threshold can be dynamically adjusted according to the robot's actual motion state; for example, the fault tolerance threshold can be appropriately relaxed during high-speed motion phases and tightened during low-speed precision operation phases. The real-time performance of the pose deviation monitoring system is ensured by a dedicated real-time task thread, which runs on a real-time operating system or a general-purpose operating system with real-time extension capabilities. The monitoring thread has a higher priority than the trajectory generation and joint servo control threads, but lower than the safety monitoring thread, ensuring that pose deviation detection is timely without affecting system safety functions. Within each control cycle, the monitoring thread sequentially performs the following operations: reads the current values ​​of all joint encoders and converts them into actual angles; reads the current data from the inertial measurement unit and calculates the actual position; obtains the theoretical position and theoretical angle at the current moment from the trajectory generator; calculates the Euclidean distance and the absolute error of each joint; compares the calculation results with the tolerance threshold; if any comparison result exceeds the limit, a pose deviation exceedance flag is set and an interrupt is triggered. Optionally, the monitoring system can record historical pose deviation data for subsequent performance analysis and system diagnostics. Optionally, the monitoring system can implement a two-tiered mechanism of soft and hard thresholds. When the deviation exceeds the lower soft threshold, an early warning is issued without interrupting the motion; only when the deviation exceeds the higher hard threshold is trajectory replanning triggered.

[0040] See Figure 5This chart demonstrates the real-time monitoring results of pose deviations during robot motion trajectory execution. Actual data collected by the inertial measurement unit and joint encoders are compared with the theoretical trajectory to comprehensively evaluate the robot's trajectory tracking performance. The chart includes multiple monitoring indicators: the solid blue line represents the position deviation of the end effector in three-dimensional space, i.e., the Euclidean distance between the actual and theoretical positions; the dashed lines of different colors represent the angle tracking errors of the four joints, reflecting the tracking accuracy of each joint actuator towards the theoretical angle. Two horizontal threshold lines in the chart set the tolerance limits for position and angle, respectively. When the monitored data exceeds these thresholds, the system triggers a trajectory replanning mechanism. The red shaded areas indicate the time periods when deviations exceed the limits; these areas are of great significance for evaluating the robot's motion safety and control performance.

[0041] Example 5: In specific implementation, triggering trajectory replanning is an emergency handling process in response to pose deviation exceeding limits. Freezing the robot's current motion state is the first step in initiating replanning. The freezing operation is achieved by sending emergency stop commands to the servo drives of all joints. The emergency stop commands cause the drives to maintain their current positions and enter a zero-force hold mode. In the frozen state, the robot's joints maintain their current angles, but the driving force is released, which prevents collisions or damage caused by continuing to execute erroneous trajectories. Acquiring the latest environmental grid map is accomplished by reactivating the environmental perception sensors. The LiDAR or depth camera is triggered to perform a one-time rapid scan, acquiring 3D point cloud data of the current working environment. The 3D point cloud data is immediately sent to the spatial rasterization processing pipeline to generate an environmental grid map reflecting the latest environmental state as quickly as possible. This processing may employ simplified algorithms to reduce computational latency. The path search space is reinitialized starting from the robot's current position, which is obtained in real time from the joint encoders and forward kinematics calculations, serving as the starting point for the new path planning. The initialization process for the path search space is similar to that of the initial planning, but the search range is typically narrowed down to a local area centered on the current location to reduce computational complexity. A shortened prediction time window is used to regenerate the optimized path; the length of the prediction time window is reduced from several seconds in the initial planning to less than one second, thus reducing the uncertainty in predicting the future movement of dynamic obstacles.

[0042] In practice, prioritizing the retention of unexecuted segments of the original optimized path in the path search space is a strategy to improve replanning efficiency. Unexecuted segments are the sequence of path points from the current time point to the endpoint of the original planned path. Retention decisions are based on obstacle verification of the spatial region where the unexecuted segment is located. By querying the latest environmental grid map, it is checked whether the grids surrounding the path points of the unexecuted segment are free space. If the unexecuted segment does not overlap with the obstacle space in the environmental grid map, the entire path segment is retained as the latter half of the new path. Verifying the dynamic obstacle avoidance constraints of the executed segments of the original optimized path is a safety check. Executed segments are historical path segments from the path start point to the current position. The verification process compares the executed segments with the predicted obstacle-occupied areas in the latest environmental grid map, calculating the minimum distance between the path segment and the obstacle area. If verification fails, a greedy algorithm is used to generate a local detour path. The greedy algorithm is a heuristic search method that selects the currently optimal local direction at each planning step. When generating local detour paths, the greedy algorithm starts from the robot's current position and uses the reconnection to a junction on the unexecuted segment as a temporary goal, gradually expanding the path points. The generation of local detour paths considers the robot's instantaneous kinematic constraints to ensure path feasibility. The local detour path is then smoothly concatenated with the unexecuted segment and output to the trajectory generator. This smooth concatenation employs curve fitting technology, inserting transition curves at the connection points between the detour path and the unexecuted segment to eliminate sharp turns.

[0043] In practical implementation, freezing the robot's current motion state involves saving the motion parameters of each joint, including position, velocity, and acceleration values, as part of the initial state for replanning. Acquiring the latest environmental grid map may involve multi-sensor data fusion, such as combining information from LiDAR and vision sensors to generate a more complete environmental representation. When reinitializing the path search space, the discretization granularity of the search space can be adjusted based on available computing resources; a coarser granularity may be used in emergency replanning to accelerate the search. When the greedy algorithm generates local detour paths, the heuristic function evaluating the merits of local directions typically includes factors such as proximity to the target, distance to obstacles, and magnitude of direction change. Optionally, local detour path generation can consider various strategies, such as traveling along obstacle boundaries or directly traversing known safe zones. Optionally, the smooth stitching process can employ different curve fitting methods, such as Bézier curves or B-spline curves, to meet different smoothness requirements. Optionally, the replanning process can include a timeout mechanism; if a feasible new path cannot be generated within a specified time, a higher-level safety strategy is initiated, such as a complete shutdown of operations awaiting human intervention.

[0044] In practical implementation, the real-time performance of the trajectory replanning process is ensured through multi-threaded parallel processing. One thread is responsible for the rapid acquisition and processing of environmental data, while another thread is dedicated to path search and optimization. The two threads exchange data via shared memory, ensuring that the latest environmental information is promptly used for path decision-making. The complexity of the replanning algorithm is controlled by limiting the search depth and branching factor, ensuring a solution can be derived within a finite timeframe. The selection of junction points between local detour paths and unexecuted segments considers the consistency of path direction, choosing points that minimize changes in the overall path direction. During smooth stitching, the curvature of the transition curve needs to be verified to meet robot kinematic constraints; if the curvature is too large, the junction point position needs to be adjusted or the detour path regenerated. After replanning is complete, the trajectory generator needs to quickly generate new joint motion trajectories. This process may employ simplified dynamic models to reduce computational burden.

[0045] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0046] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A robot path control method based on automated operation, characterized in that, Includes the following steps: Acquire 3D point cloud data of the robot's current working environment, perform spatial rasterization processing on the 3D point cloud data, and generate an environmental raster map; Dynamic obstacle trajectories are extracted from environmental grid maps, and dynamic obstacle avoidance constraints are constructed by combining robot kinematic parameters. The path search space is initialized based on the dynamic obstacle avoidance constraints, and a set of candidate paths is generated in the path search space using a heuristic strategy. For each path in the candidate path set, perform smoothness and energy consumption evaluations, and select the optimized path that meets the preset evaluation threshold. The optimized path is input into the trajectory generator to generate the continuous motion trajectory of each joint of the robot; The robot monitors its pose deviation in real time when executing its motion trajectory. If the pose deviation exceeds the fault tolerance threshold, trajectory replanning is triggered.

2. The robot path control method based on automated operation according to claim 1, characterized in that, The spatial rasterization process for the 3D point cloud data includes: Set the raster resolution parameters and divide the 3D point cloud data into cubic units of equal volume according to the resolution parameters; Calculate the point cloud density within each cubic cell, and mark cells with a density below a first threshold as free space and cells with a density above a second threshold as obstacle space; Morphological closing operations are performed on adjacent obstacle spatial units to generate connected region labels.

3. The robot path control method based on automated operation according to claim 2, characterized in that, The dynamic obstacle avoidance constraints include: Extract the continuously changing sequence of obstacle spatial units in the connected region labels and calculate the displacement vector of the center point of the sequence; Predict the area occupied by dynamic obstacles within a future time window based on displacement vectors; By combining the robot's body radius and maximum deceleration, the minimum safe distance between the area occupied by dynamic obstacles and the robot's path is calculated.

4. The robot path control method based on automated operation according to claim 3, characterized in that, The generated candidate path set includes: Select the line connecting the robot's current position and the target position in the environmental grid map as the reference direction; A detection ray is emitted at a preset angle interval along the reference direction. When the detection ray encounters an obstacle space, a path turning point is generated. Match the path turning points with the maximum curvature constraints in the robot's kinematic model, and eliminate combinations of turning points that do not meet the curvature constraints.

5. The robot path control method based on automated operation according to claim 4, characterized in that, The smoothness evaluation includes: calculating the rate of change of steering angle between adjacent path segments in the candidate path, counting the number of path segments whose rate of change of steering angle exceeds the critical value, and using the proportion of the number as the smoothness score. The energy consumption assessment includes: calculating the joint torque integral required for each path segment based on the robot mass distribution model, and using the ratio of the torque integral to the path length as the energy consumption score.

6. The robot path control method based on automated operation according to claim 5, characterized in that, The optimized paths that meet the preset evaluation threshold include: The smoothness score and energy consumption score are weighted and fused to generate a comprehensive path score; The candidate path with the best comprehensive path score is selected as the optimized path; If the overall path score of all candidate paths fails to reach the preset evaluation threshold, the smoothness score weight will be reduced and the path will be re-selected.

7. The robot path control method based on automated operation according to claim 6, characterized in that, The generated continuous motion trajectories of each joint of the robot include: The optimized path is discretized into a sequence of path points with equal time intervals; Perform cubic spline interpolation on the path point sequence to generate the position-time function in the joint space; Based on the first and second derivatives of the position-time function, joint velocity curves and acceleration curves are generated respectively.

8. The robot path control method based on automated operation according to claim 7, characterized in that, The real-time monitoring of the robot's pose deviation when executing its motion trajectory includes: The Euclidean distance between the actual position and the theoretical position is obtained through the inertial measurement unit of the robot's end effector; The absolute error between the actual angle and the theoretical angle of each joint is obtained by using a joint encoder; When the Euclidean distance or the absolute error of any joint exceeds the tolerance threshold, it is determined that the pose deviation exceeds the limit.

9. The robot path control method based on automated operation according to claim 8, characterized in that, The triggered trajectory replanning includes: Freeze the robot's current motion state and obtain the latest environmental grid map; Reinitialize the path search space starting from the robot's current position; The optimized path is regenerated using a shortened prediction time window.

10. The robot path control method based on automated operation according to claim 9, characterized in that, The regenerated optimized path includes: In the path search space, prioritize retaining the unexecuted segments of the original optimized path; The executed segments of the original optimized path are dynamically checked for obstacle avoidance constraints. If the check fails, a greedy algorithm is used to generate a local detour path. The local detour path is smoothly spliced ​​with the unexecuted segment and then output to the trajectory generator.

Images