Robot safety trajectory planning analysis method considering obstacle avoidance stability

Abstract

The invention relates to the technical field of artificial intelligence and automation, in particular to a robot safety trajectory planning analysis method considering obstacle avoidance stability, which comprises the following steps of: representing a robot operation environment by adopting an occupied grid, collecting an obstacle set, setting a robot driving target point, and ensuring that a robot accurately recognizes obstacles; the driving starting point and the driving path of the robot are defined according to the occupied grid, the initial track is generated, the shortest path is searched, and the driving efficiency is improved; by optimizing the preliminary trajectory, setting obstacle avoidance constraint conditions and adjusting the trajectory to avoid collision, the adaptability and robustness of the robot are improved; and a control instruction is generated according to the adjusted track and sent to the robot for execution, so that the feedback capability and autonomy of the robot are improved. The method is used for solving the technical problems of inaccurate track planning and unstable obstacle avoidance of robot navigation in the existing scheme.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence and automation technology, and in particular to a robot safety trajectory planning and analysis method taking obstacle avoidance stability into consideration. Background Art

[0002] Artificial intelligence refers to a technology that simulates human intelligence through computer technology. Automation refers to the use of computers, robots, sensors and other technologies to automatically control and manage various links such as production, manufacturing, and sales, thereby improving production efficiency and quality.

[0003] There are still some problems in the current robot navigation and control in the field of artificial intelligence and automation technology, including the following aspects: inaccurate trajectory planning. The current technology fails to fully consider the complexity of the environment and the diversity of obstacles when planning the robot's trajectory, resulting in the planned trajectory being not accurate enough, and it is easy to collide with obstacles or deviate from the predetermined path; insufficient obstacle avoidance stability. When facing dynamically changing obstacles and complex environments, the obstacle avoidance strategy of the current technology may lack sufficient stability, causing the robot to jitter or become unstable during the obstacle avoidance process, affecting the robot's operating efficiency and safety; lack of optimization mechanism. The current technology may lack optimized management of the robot's driving trajectory, and cannot reduce unnecessary acceleration changes and jitters while meeting obstacle avoidance stability, thereby affecting the robot's driving stability; to address these problems, corresponding solutions and technical means need to be adopted to enhance the robot's adaptability and flexibility. Summary of the invention

[0004] The purpose of the present invention is to solve the problems in the background technology and to propose a robot safety trajectory planning and analysis method taking obstacle avoidance stability into consideration.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] A robot safety trajectory planning analysis method considering obstacle avoidance stability includes:

[0007] Step 1: Use the occupied grid to represent the robot's operating environment, collect the obstacle set, and set the robot's driving target point;

[0008] Step 2: define the robot's driving starting point and driving path based on the occupied grid, process the driving path to generate a preliminary trajectory of the robot, and search for the shortest path between the starting point and the target point based on the preliminary trajectory of the robot;

[0009] Step 3: Optimize the robot's initial trajectory and set obstacle avoidance constraints to determine whether the optimized robot's trajectory collides with obstacles, thereby adjusting the robot's trajectory.

[0010] Step 4: Generate control instructions based on the adjusted robot driving trajectory, send the control instructions to the robot for execution, and the robot replans the driving trajectory according to the control instructions and completes the obstacle avoidance operation.

[0011] It should be noted that the robot safety trajectory planning and analysis method considering obstacle avoidance stability proposed in the present invention can be widely applicable to path planning and safety control in robot navigation. The method can be used to improve the robot's obstacle avoidance ability and driving stability in a dynamic environment. Specifically, it can be through occupying grid representation, polynomial interpolation and other technologies to comprehensively analyze and accurately evaluate the robot's driving trajectory. Through path search, trajectory generation, obstacle avoidance stability analysis and optimization and other links, the smoothness, safety and stability of the robot's driving trajectory are ensured, which promotes the practical application of autonomous driving technology and the intelligence of robot navigation technology. At the same time, the method meets the needs of safe and efficient operation of robots in complex environments, and achieves a significant improvement in the robot's autonomous navigation and obstacle avoidance capabilities.

[0012] Furthermore, the process of using the occupied grid to represent the robot's operating environment, collecting the obstacle set, and setting the robot's driving target point includes:

[0013] In the robot operating environment, an occupied grid is used for representation: the robot operating environment is divided into an occupied grid G ​​of τ×ι; wherein τ×ι represents the size of the occupied grid, that is, the occupied grid has τ rows and ι columns;

[0014] Each grid cell c(i, j) in the occupied grid corresponds to a position in the robot's operating environment: if the value of the grid cell c(i, j) is set to 0, it means that the position is idle, that is, it is not occupied by any obstacle; conversely, if the value of the grid cell c(i, j) is set to 1, it indicates that the position is occupied by an obstacle; where i is the row index, j is the column index, and 1≤i≤τ,1≤j≤ι;

[0015] Define the obstacle set as o = {o1, o2, ..., o e}, where e represents the number of obstacles; each obstacle is defined by a set of polygons or points;

[0016] Identify obstacles in the robot's operating environment and map them to corresponding grid cells on the occupied grid G, i.e. c(i,j)∈o;

[0017] Add the grid cells occupied by obstacles to the obstacle set o;

[0018] Select a grid cell on the occupied grid G ​​that is not occupied by obstacles as the target point P for the robot to travel;

[0019] Determine the coordinates P(x, y) of the target point of the robot's travel, and ensure that the grid cell corresponding to the coordinates is reachable (that is, not in the obstacle set o); where x and y are the row and column positions of the target point of the robot's travel in the grid G, respectively.

[0020] Furthermore, the process of defining the robot's driving starting point and driving path based on the occupied grid and processing the driving path to generate a preliminary trajectory of the robot's driving includes:

[0021] In the occupied grid G, define the starting point of the robot as P s =(x s ,y s ), where s represents the starting point index, which is used to distinguish the target point P of the robot;

[0022] The robot's driving path R represents a collection of grid cells.

[0023] That is, R = {c(m1,n1),c(m2,n2),...,c(m k-1 ,n k-1 )}, where c(m1,n1) represents the starting point P of the robot. s The grid cell where it is located, c(m k-1, n k-1 ) represents the grid unit where the target point P of the robot is located, and k represents the total number of grid units in the path;

[0024] The robot's driving path R is interpolated to generate a smooth preliminary robot driving trajectory T(t) = (x(t), y(t)), where T(t) is a function representation of the preliminary robot driving trajectory, x(t) and y(t) represent the components of the robot's driving trajectory on the x and y axes, respectively, and t represents a time parameter, which is used to describe the change of the position of each point on the robot's driving trajectory over time;

[0025] The interpolation process is as follows: Assume that there is a set of discrete driving trajectory points (x1, y1), (x2, y2), ..., (x q ,y q ), represents the position of the robot at different time points, where q represents the number of driving trajectory points;

[0026] Using the polynomial I(x) = μ0+μ1x+μ2x 2 +…+μ L x L Approaching discrete driving trajectory points, where μ0,μ1,...,μ L represents the coefficient of the polynomial, and L represents the degree of the polynomial;

[0027] Use the least squares method to find a set of coefficients that minimizes the sum of squared errors of the polynomial I(x) at all discrete driving trajectory points.

[0028] Right now In the formula, r represents the index of the driving trajectory point;

[0029] The minimized polynomial I(x) is time-mapped to obtain the robot's initial trajectory T(t) = (x(t), y(t));

[0030] It can be understood that the interpolation method can ensure that the robot's driving trajectory transitions smoothly between trajectory points.

[0031] Furthermore, the process of searching for the shortest path between the starting point and the target point according to the preliminary trajectory of the robot includes:

[0032] According to the generated preliminary trajectory of the robot, search for the starting point P of the robot in the occupied grid G. s The shortest path to the target point P, the specific analysis process is as follows:

[0033] According to the grid cell c(i,j) in the occupied grid, a distance array Dis[i,j] is set to indicate a specific position in the occupied grid G;

[0034] Initialize the distance array Dis[P s .x,P s .y]=0, indicating the starting point P of the robot s The distance to itself is 0;

[0035] For all other robot driving positions c(i,j)≠Dis[P s .x,P s .y], set Dis[i,j] = ∞, indicating that the shortest path has not been found;

[0036] Create a set of robot driving positions to be processed Fro, which initially only contains the robot's starting point P s , and perform the following steps:

[0037] Z1, randomly select a robot driving position (x, y) from the robot driving position set Fro to be processed, so that the Dis[x, y] value is the smallest;

[0038] Z2. For all neighbor positions (x', y') of (x, y), if Dis[x, y]+1<Dis[x', y'], then update Dis[x', y']=Dis[x, y]+1, and add (x', y') to the robot driving position set Fro to be processed;

[0039] Z3, when the robot's target point P is processed, the iteration ends. At this time, Dis[P s .x,P s .y] is the starting point P of the robot s The shortest path to the destination point P.

[0040] Furthermore, the preliminary trajectory of the robot is optimized, and obstacle avoidance constraints are set to determine whether the optimized robot trajectory collides with an obstacle. The process of adjusting the robot trajectory includes:

[0041] Get the robot's initial trajectory T(t);

[0042] The robot's initial trajectory T(t) is optimized to reduce jitter and unnecessary acceleration changes, and T opt (t) = (x opt (t),y opt (t)), where T opt (t) is the optimized robot trajectory function, which changes with time t, x opt (t), y opt (t) represents the components of the optimized robot's trajectory on the x and y axes respectively;

[0043] Set obstacle avoidance constraints, including robot obstacle avoidance stability constraints, robot driving speed constraints, and robot driving acceleration constraints:

[0044] Let the position set of obstacles on the two-dimensional plane be Among them, h represents the obstacle index, e represents the number of obstacles, (x h ,y h ) represents the hth obstacle position;

[0045] Optimized robot driving trajectory T opt (t) Satisfy the condition of not intersecting with all obstacles, that is,

[0046]

[0047] Where t0, t f They represent the start time and end time of the robot's driving trajectory, δ represents the safety distance, and it is required that the distance between the robot's driving trajectory and the obstacle at any time is greater than δ;

[0048] Let the maximum limit of the robot's speed be v max , then the optimized robot driving trajectory T opt (t)Satisfy:

[0049]

[0050] In the formula, Respectively represent the velocity components of the optimized robot's driving trajectory on the x-axis and y-axis;

[0051] Let the maximum limit of the robot's driving acceleration be a max , then the optimized robot driving trajectory T opt (t)Satisfy:

[0052]

[0053] In the formula, Respectively represent the acceleration components of the optimized robot's driving trajectory on the x-axis and y-axis;

[0054] Get the obstacle set o and check the optimized robot trajectory T opt (t) Whether a collision with an obstacle occurs: If a collision occurs, the robot's driving trajectory is replanned; the robot's driving trajectory planning method includes replanning the path, modifying interpolation parameters, or applying obstacle avoidance strategies (such as emergency braking, detour, etc.).

[0055] Furthermore, a control instruction is generated according to the adjusted robot driving trajectory, the control instruction is sent to the robot for execution, the robot replans the driving trajectory according to the control instruction, and the process of completing the obstacle avoidance operation includes:

[0056] Get the adjusted robot driving trajectory T adj (t), that is, T adj (t) = (x adj (t),y adj (t)), where T adj (t) is the adjusted robot trajectory function, which changes with time t, x adj (t), y adj (t) represents the components of the robot’s trajectory after adjustment on the x and y axes respectively;

[0057] According to the adjusted robot driving trajectory T adj (t) Generate control instructions: By calculating the adjusted robot trajectory T adj The derivative of (t) obtains the desired velocity and acceleration information:

[0058]

[0059] In the formula, are the velocity and acceleration vectors of the robot's trajectory, representing the robot's velocity and acceleration at time t, i.e., the expected velocity and acceleration, Respectively represent the velocity components of the robot's trajectory on the x-axis and y-axis after adjustment, Respectively represent the acceleration components of the robot's trajectory after adjustment on the x-axis and y-axis;

[0060] Sending the generated control instructions to the robot for execution; wherein the control instructions include the desired speed and acceleration information;

[0061] The robot plans its driving trajectory according to the control instructions it receives, including adjusting speed, steering angle, etc.

[0062] During the movement, the robot will constantly monitor changes in the surrounding environment (such as the appearance of new obstacles, changes in road conditions, etc.) and the execution of the trajectory (such as whether it deviates from the predetermined trajectory, whether the speed requirements are met, etc.).

[0063] Compared with the existing technology, the present invention provides a robot safety trajectory planning and analysis method considering obstacle avoidance stability, which has the following advantages:

[0064] 1. The present invention uses the grid to represent the robot's operating environment, collects obstacle sets, and sets the robot's driving target points. By occupying the grid, the complex robot's operating environment is simplified into a discrete representation that is easy for computer processing, which is convenient for the implementation of subsequent path search and trajectory planning algorithms. At the same time, collecting obstacle sets and setting driving target points provide clear goals and constraints for subsequent trajectory planning;

[0065] 2. The present invention defines the starting point and driving path of the robot on the basis of occupying the grid, and generates a smooth preliminary trajectory through interpolation and other methods, thereby ensuring the continuity and smoothness of the robot's driving trajectory, reducing the jitter and unnecessary acceleration changes caused by sudden changes in the trajectory, and improving the comfort and stability of the robot's driving;

[0066] 3. The present invention optimizes the initial driving trajectory of the robot and sets obstacle avoidance constraints to determine whether the optimized driving trajectory of the robot collides with an obstacle, thereby adjusting the driving trajectory of the robot, avoiding potential collision risks, and ensuring the stable operation of the robot; control instructions are generated according to the optimized driving trajectory and sent to the robot for execution, thereby achieving precise control of the robot's driving process, improving the robot's autonomous navigation capability, and ensuring the smooth completion of the task.

[0067] In summary, the present invention, through the interrelationships and mutual support of the above steps, together constitutes a complete and efficient robot safety trajectory planning and analysis method, which has significant beneficial effects in improving the safety and stability of robot driving, and ensures the normal implementation of a subsequent robot safety trajectory planning and analysis method that considers obstacle avoidance stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 The present invention provides a flowchart of a robot safety trajectory planning and analysis method taking into account obstacle avoidance stability. DETAILED DESCRIPTION

[0069] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the implementation regulations described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0070] Reference Figure 1 , a robot safety trajectory planning analysis method considering obstacle avoidance stability, including:

[0071] Step 1: Use the occupied grid to represent the robot's operating environment, collect the obstacle set, and set the robot's driving target point;

[0072] Step 2: define the robot's driving starting point and driving path based on the occupied grid, process the driving path to generate a preliminary trajectory of the robot, and search for the shortest path between the starting point and the target point based on the preliminary trajectory of the robot;

[0073] Step 3: Optimize the robot's initial trajectory and set obstacle avoidance constraints to determine whether the optimized robot's trajectory collides with obstacles, thereby adjusting the robot's trajectory.

[0074] Step 4: Generate control instructions based on the adjusted robot driving trajectory, send the control instructions to the robot for execution, and the robot replans the driving trajectory according to the control instructions and completes the obstacle avoidance operation.

[0075] It should be noted that the robot safety trajectory planning and analysis method considering obstacle avoidance stability proposed in the present invention can be widely applicable to path planning and safety control in robot navigation. The method can be used to improve the robot's obstacle avoidance ability and driving stability in a dynamic environment. Specifically, it can be through occupying grid representation, polynomial interpolation and other technologies to comprehensively analyze and accurately evaluate the robot's driving trajectory. Through path search, trajectory generation, obstacle avoidance stability analysis and optimization and other links, the smoothness, safety and stability of the robot's driving trajectory are ensured, which promotes the practical application of autonomous driving technology and the intelligence of robot navigation technology. At the same time, the method meets the needs of safe and efficient operation of robots in complex environments, and achieves a significant improvement in the robot's autonomous navigation and obstacle avoidance capabilities.

[0076] See also Figure 1The present invention provides a robot safety trajectory planning and analysis method considering obstacle avoidance stability. In step 1, the steps of using an occupied grid to represent the robot operating environment, collecting an obstacle set, and setting the robot driving target point include:

[0077] Step 101, in the robot operating environment, using an occupied grid for representation: the robot operating environment is divided into an occupied grid G ​​of τ×ι; wherein τ×ι represents the size of the occupied grid, that is, the occupied grid has τ rows and ι columns;

[0078] Step 102: Each grid cell c(i, j) in the occupied grid corresponds to a position in the robot operating environment: if the value of the grid cell c(i, j) is set to 0, it means that the position is idle, that is, it is not occupied by any obstacle; conversely, if the value of the grid cell c(i, j) is set to 1, it indicates that the position is occupied by an obstacle; wherein i is a row index, j is a column index, and 1≤i≤τ, 1≤j≤ι;

[0079] Step 103: define the obstacle set as o = {o1, o2, ..., o e}, where e represents the number of obstacles; each obstacle is defined by a set of polygons or points;

[0080] Step 104: Identify obstacles in the robot's operating environment and map them to corresponding grid cells on the occupied grid G, i.e., c(i,j)∈o;

[0081] Step 105: add the grid cells occupied by the obstacles to the obstacle set o;

[0082] Step 106: Select a grid cell on the occupied grid G ​​that is not occupied by an obstacle as the target point P for the robot to travel;

[0083] Step 107: determine the coordinates P(x, y) of the target point of the robot, and ensure that the grid cell corresponding to the coordinates is reachable (i.e., not in the obstacle set o); where x and y are the row and column positions of the target point of the robot in the grid G ​​respectively;

[0084] In steps 101-107, the occupied grid can be used to clearly divide the robot's operating environment, so that each position in the environment has a corresponding grid unit, which is convenient for subsequent processing and analysis; by mapping obstacles to the occupied grid, the positions of obstacles in the environment can be quickly identified so that the robot's driving path can be planned later to avoid collisions; a grid unit that is not occupied by obstacles is selected on the occupied grid as the target point for the robot's driving, which ensures the accessibility of the target point and improves the effectiveness of path planning.

[0085] See also Figure 1 The present invention provides a robot safety trajectory planning and analysis method considering obstacle avoidance stability. In the step 2, the robot driving starting point and driving path are defined based on the occupied grid, the driving path is processed to generate a preliminary trajectory of the robot, and the shortest path between the starting point and the target point is searched according to the preliminary trajectory of the robot. The steps include:

[0086] Step 201: In the occupied grid G, define the starting point of the robot as P s =(x s ,y s ), where s represents the starting point index, which is used to distinguish the target point P of the robot;

[0087] Step 202: Define the weight matrix W∈R of the high-level language model u×(b1+b2+b3) ;

[0088] In step 202, the robot driving path R represents a collection of a series of grid cells.

[0089] That is, R = {c(m1,n1),c(m2,n2),...,c(m k-1 ,n k-1 )}, where c(m1,n1) represents the starting point P of the robot. s The grid cell where it is located, c(m k-1 ,n k-1 ) represents the grid unit where the target point P of the robot is located, and k represents the total number of grid units in the path;

[0090] Step 203: interpolate the robot's driving path R to generate a smooth preliminary robot driving trajectory T(t)=(x(t), y(t)), where T(t) is a function representation of the preliminary robot driving trajectory, x(t) and y(t) represent the components of the robot driving trajectory on the x and y axes respectively, and t represents a time parameter, which is used to describe the change of the position of each point on the robot driving trajectory over time;

[0091] In step 203, the interpolation process is as follows: Assume that there is a set of discrete driving trajectory points (x1, y1), (x2, y2), ..., (x q ,y q ), represents the position of the robot at different time points, where q represents the number of driving trajectory points;

[0092] Using the polynomial I(x) = μ0+μ1x+μ2x 2 +…+μ L x L Approaching discrete driving trajectory points, where μ0,μ1,...,μL represents the coefficient of the polynomial, and L represents the degree of the polynomial;

[0093] Use the least squares method to find a set of coefficients that minimizes the sum of squared errors of the polynomial I(x) at all discrete driving trajectory points.

[0094] Right now In the formula, r represents the index of the driving trajectory point;

[0095] The minimized polynomial I(x) is time-mapped to obtain the robot's initial trajectory T(t) = (x(t), y(t));

[0096] Step 204: Based on the generated preliminary trajectory of the robot, search for the starting point P of the robot in the occupied grid G. s The shortest path to the target point P, the specific analysis process is as follows:

[0097] According to the grid cell c(i,j) in the occupied grid, a distance array Dis[i,j] is set to indicate a specific position in the occupied grid G;

[0098] Initialize the distance array Dis[P s .x,P s .y]=0, indicating the starting point P of the robot s The distance to itself is 0;

[0099] For all other robot driving positions c(i,j)≠Dis[P s .x,P s .y], set Dis[i,j] = ∞, indicating that the shortest path has not been found;

[0100] Create a set of robot driving positions to be processed Fro, which initially only contains the robot's starting point P s , and perform the following steps:

[0101] Z1, randomly select a robot driving position (x, y) from the robot driving position set Fro to be processed, so that the Dis[x, y] value is the smallest;

[0102] Z2. For all neighbor positions (x', y') of (x, y), if Dis[x, y]+1<Dis[x', y'], then update Dis[x', y']=Dis[x, y]+1, and add (x', y') to the robot driving position set Fro to be processed;

[0103] Z3, when the robot's target point P is processed, the iteration ends. At this time, Dis[P s .x,Ps .y] is the starting point P of the robot s The shortest path to the target point P;

[0104] In steps 201-204, the interpolation method is used to ensure that the robot's driving trajectory transitions smoothly between trajectory points; based on the generated preliminary trajectory, the shortest path from the starting point to the target point is searched in the occupied grid. This method not only considers the length of the path, but also the accessibility and obstacle avoidance of the path, thereby ensuring that the robot can reach the destination in the shortest and safest way.

[0105] See also Figure 1 The present invention provides a robot safety trajectory planning and analysis method considering obstacle avoidance stability. In step three, the robot's preliminary driving trajectory is optimized, and obstacle avoidance constraints are set. It is determined whether the optimized robot's driving trajectory collides with an obstacle, and the steps of adjusting the robot's driving trajectory include:

[0106] Step 301, obtaining the robot's initial trajectory T(t);

[0107] Step 302: Optimize the robot's initial trajectory T(t) to reduce vibration and unnecessary acceleration changes, and obtain T opt (t) = (x opt (t),y opt (t)), where T opt (t) is the optimized robot trajectory function, which changes with time t, x opt (t), y opt (t) represents the components of the optimized robot's trajectory on the x and y axes respectively;

[0108] Step 303: setting obstacle avoidance constraints, including robot obstacle avoidance stability constraints, robot driving speed constraints, and robot driving acceleration constraints:

[0109] Let the position set of obstacles on the two-dimensional plane be Among them, h represents the obstacle index, e represents the number of obstacles, (x h ,y h ) represents the hth obstacle position;

[0110] Optimized robot driving trajectory T opt (t) Satisfy the condition of not intersecting with all obstacles, that is,

[0111]

[0112] Where t0, t fThey represent the start time and end time of the robot's driving trajectory, δ represents the safety distance, and it is required that the distance between the robot's driving trajectory and the obstacle at any time is greater than δ;

[0113] Let the maximum limit of the robot's speed be v max , then the optimized robot driving trajectory T opt (t)Satisfy:

[0114]

[0115] In the formula, Respectively represent the velocity components of the optimized robot's driving trajectory on the x-axis and y-axis;

[0116] Let the maximum limit of the robot's driving acceleration be a max , then the optimized robot driving trajectory T opt (t)Satisfy:

[0117]

[0118] In the formula, Respectively represent the acceleration components of the optimized robot's driving trajectory on the x-axis and y-axis;

[0119] Step 304: Obtain the obstacle set o and check the optimized robot driving trajectory T opt (t) Whether there is a collision with an obstacle: If a collision occurs, the robot's driving trajectory is replanned; the robot's driving trajectory planning method includes replanning the path, modifying interpolation parameters, or applying obstacle avoidance strategies (such as emergency braking, detour, etc.);

[0120] In steps 301-304, by comprehensively considering factors such as the robot's driving trajectory, obstacle position, driving speed and acceleration, the robot's driving trajectory is comprehensively and accurately planned, thereby improving the accuracy and reliability of trajectory planning; by setting obstacle avoidance constraints, including robot obstacle avoidance stability constraints, driving speed constraints and acceleration constraints, it is ensured that the robot can effectively avoid collisions with obstacles during driving, thereby improving obstacle avoidance stability;

[0121] See also Figure 1 The present invention provides a robot safety trajectory planning and analysis method considering obstacle avoidance stability. In step 4, a control instruction is generated according to the adjusted robot driving trajectory, the control instruction is sent to the robot for execution, the robot replans the driving trajectory according to the control instruction, and the step of completing the obstacle avoidance operation includes:

[0122] Step 401: Obtain the adjusted robot driving trajectory T adj (t), that is, Tadj (t) = (x adj (t),y adj (t)), where T adj (t) is the adjusted robot trajectory function, which changes with time t, x adj (t), y adj (t) represents the components of the robot’s trajectory after adjustment on the x and y axes respectively;

[0123] Step 402: According to the adjusted robot driving trajectory T adj (t) Generate control instructions: By calculating the adjusted robot trajectory T adj The derivative of (t) obtains the desired velocity and acceleration information:

[0124]

[0125] In the formula, are the velocity and acceleration vectors of the robot's trajectory, representing the robot's velocity and acceleration at time t, i.e., the expected velocity and acceleration, Respectively represent the velocity components of the robot's trajectory on the x-axis and y-axis after adjustment, Respectively represent the acceleration components of the robot's trajectory after adjustment on the x-axis and y-axis;

[0126] Step 403: Send the generated control instruction to the robot for execution; wherein the control instruction includes the desired speed and acceleration information;

[0127] Step 404: The robot plans the driving trajectory according to the received control command, including adjusting the speed, steering angle, etc.;

[0128] Step 405: During the movement, the robot will continuously monitor the changes in the surrounding environment (such as the appearance of new obstacles, changes in road conditions, etc.) and the execution of the trajectory (such as whether it deviates from the predetermined trajectory, whether the speed requirement is met, etc.);

[0129] In steps 401-405, by dynamically adjusting the driving trajectory of the robot, it is possible to flexibly respond to obstacles in different environments and ensure the driving safety of the robot; by generating control instructions containing expected speed and acceleration information, the robot can quickly and accurately perform obstacle avoidance operations and improve driving efficiency.

[0130] In the embodiment of the present invention, by dividing the robot operating environment into occupied grids, each grid unit corresponds to a position in the operating environment, and the occupied grid representation method can describe the robot operating environment concisely and clearly, which is convenient for subsequent processing. By interpolation processing, it can be ensured that the robot's driving trajectory smoothly transitions between trajectory points, thereby improving the stability and comfort of driving. By searching for the shortest path from the robot's driving starting point to the target point, it is ensured that the robot reaches the target point with the shortest distance during driving, thereby improving driving efficiency. By using the iterative update method of the distance array and the set of positions to be processed, the shortest path can be effectively searched, thereby improving the calculation efficiency. By setting conditions such as obstacle avoidance stability constraints, driving speed constraints, and driving acceleration constraints, it is ensured that the robot's driving trajectory maintains a certain safe distance from obstacles. By calculating the expected speed and acceleration information, a control instruction is generated so that the robot drives according to the expected trajectory and speed, thereby improving the accuracy of control. During driving, the robot continuously monitors changes in the surrounding environment and the execution of the trajectory, and replans the driving trajectory and generates control instructions based on the perception information, thereby ensuring that timely adjustments can be made when encountering new obstacles or environmental changes, thereby improving the feedback capability and autonomy of the robot. In summary, the examples of the present invention involve data processing, comprehensive analysis and intelligent optimization decision-making to solve the technical problems of inaccurate trajectory planning and unstable obstacle avoidance of robot navigation in existing solutions. In actual situations, more data and contextual information may be needed to make specific decisions and optimization solutions.

[0131] In addition, the formulas involved in the above are all calculated by removing dimensions and taking their numerical values. They are a formula that is closest to the actual situation obtained by collecting a large amount of data and performing software simulation. The proportional coefficient in the formula and the various preset thresholds in the analysis process are set by technical personnel in this field according to actual conditions or obtained by simulating a large amount of data; the size of the proportional coefficient is to quantify each parameter to obtain a specific value for subsequent comparison. The size of the proportional coefficient depends on the amount of sample data and the preliminary setting of the corresponding processing coefficient for each group of sample data by technical personnel in this field; as long as it does not affect the proportional relationship between the parameter and the quantized value.

[0132] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically based on the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0133] For the convenience of description, the above device is described in various units according to their functions. Of course, when implementing the present application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0134] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0135] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0136] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0137] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0138] Secondly: In the drawings of the embodiments disclosed in the present invention, only the structures related to the embodiments disclosed in the present invention are involved, and other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of the present invention can be combined with each other;

[0139] Finally: The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A robot safety trajectory planning and analysis method considering obstacle avoidance stability, characterized by: Step 1: Use the occupied grid to represent the robot's operating environment, collect the obstacle set, and set the robot's driving target point; Step 2: define the robot's driving starting point and driving path based on the occupied grid, process the driving path to generate a preliminary trajectory of the robot, and search for the shortest path between the starting point and the target point based on the preliminary trajectory of the robot; Step 3: Optimize the robot's initial trajectory and set obstacle avoidance constraints to determine whether the optimized robot's trajectory collides with obstacles, thereby adjusting the robot's trajectory. Step 4: Generate control instructions based on the adjusted robot driving trajectory, send the control instructions to the robot for execution, and the robot replans the driving trajectory according to the control instructions and completes the obstacle avoidance operation.

2. The robot safety trajectory planning and analysis method considering obstacle avoidance stability according to claim 1, characterized in that: In the step 1, the process of using the occupied grid to represent the robot operating environment, collecting the obstacle set, and setting the robot driving target point includes: In the robot operating environment, an occupied grid is used for representation: the robot operating environment is divided into an occupied grid G ​​of τ×ι; wherein τ×ι represents the size of the occupied grid, that is, the occupied grid has τ rows and ι columns; Each grid cell c(i, j) in the occupied grid corresponds to a position in the robot's operating environment: if the value of the grid cell c(i, j) is set to 0, it means that the position is idle, that is, it is not occupied by any obstacle; conversely, if the value of the grid cell c(i, j) is set to 1, it indicates that the position is occupied by an obstacle; where i is the row index, j is the column index, and 1≤i≤τ,1≤j≤ι; Define the obstacle set as o = {o1, o2, ..., o e }, where e represents the number of obstacles; each obstacle is defined by a set of polygons or points; Identify obstacles in the robot's operating environment and map them to corresponding grid cells on the occupied grid G, i.e. c(i,j)∈o; Add the grid cells occupied by obstacles to the obstacle set o; Select a grid cell on the occupied grid G ​​that is not occupied by obstacles as the target point P for the robot to travel; Determine the coordinates P(x,y) of the target point where the robot is traveling; where x and y are the row and column positions of the target point where the robot is traveling in the grid G, respectively.

3. The robot safety trajectory planning and analysis method considering obstacle avoidance stability according to claim 1, characterized in that: In the step 2, the process of defining the robot's driving starting point and driving path based on the occupied grid, and processing the driving path to generate the robot's preliminary driving trajectory includes: In the occupied grid G, define the starting point of the robot as P s =(x s ,y s ), where s represents the starting point index, which is used to distinguish the target point P of the robot; The robot's driving path R represents a collection of grid cells. That is, R = {c(m1,n1),c(m2,n2),...,c(m k-1 ,n k-1 )}, where c(m1,n1) represents the starting point P of the robot. s The grid cell where it is located, c(m k-1, n k-1 ) represents the grid unit where the target point P of the robot is located, and k represents the total number of grid units in the path; The robot's driving path R is interpolated to generate a smooth preliminary robot driving trajectory T(t) = (x(t), y(t)), where T(t) is a function representation of the preliminary robot driving trajectory, x(t) and y(t) represent the components of the robot's driving trajectory on the x and y axes, respectively, and t represents a time parameter, which is used to describe the change of the position of each point on the robot's driving trajectory over time; The interpolation process is as follows: Assume that there is a set of discrete driving trajectory points (x1, y1), (x2, y2), ..., (x q ,y q ), represents the position of the robot at different time points, where q represents the number of driving trajectory points; Using the polynomial I(x) = μ0+μ1x+μ2x 2 +…+μ L x L Approaching discrete driving trajectory points, where μ0,μ1,...,μ L represents the coefficient of the polynomial, and L represents the degree of the polynomial; Use the least squares method to find a set of coefficients that minimizes the sum of squared errors of the polynomial I(x) at all discrete driving trajectory points. Right now In the formula, r represents the index of the driving trajectory point; The minimized polynomial I(x) is time-mapped to obtain the robot's initial trajectory T(t) = (x(t), y(t)).

4. The robot safety trajectory planning and analysis method considering obstacle avoidance stability according to claim 3 is characterized in that: In step 2, the process of searching for the shortest path between the starting point and the target point according to the preliminary trajectory of the robot includes: According to the generated preliminary trajectory of the robot, search for the starting point P of the robot in the occupied grid G. s The shortest path to the target point P, the specific analysis process is as follows: Set a distance array Dis[i,j] according to the grid cell c(i,j) in the occupied grid; Initialize the distance array Dis[P s .x,P s .y]=0, indicating the starting point P of the robot s The distance to itself is 0; For all other robot driving positions c(i,j)≠Dis[P s .x,P s .y], set Dis[i,j] = ∞, indicating that the shortest path has not been found; Create a set of robot driving positions to be processed Fro, which initially only contains the robot's starting point P s , and perform the following steps: Z1, randomly select a robot driving position (x, y) from the robot driving position set Fro to be processed, so that the Dis[x, y] value is the smallest; Z2. For all neighbor positions (x', y') of (x, y), if Dis[x, y]+1<Dis[x', y'], then update Dis[x', y']=Dis[x, y]+1, and add (x', y') to the robot driving position set Fro to be processed; Z3, when the robot's target point P is processed, the iteration ends. At this time, Dis[P s .x,P s .y] is the starting point P of the robot s The shortest path to the destination point P.

5. The robot safety trajectory planning and analysis method considering obstacle avoidance stability according to claim 1, characterized in that: In the step 3, the preliminary trajectory of the robot is optimized, and obstacle avoidance constraints are set to determine whether the optimized trajectory of the robot collides with an obstacle, thereby adjusting the trajectory of the robot, including: Get the robot's initial trajectory T(t); Optimize the robot's initial trajectory T(t) and get T opt (t) = (x opt (t),y opt (t)), where T opt (t) is the optimized robot trajectory function, which changes with time t, x opt (t), y opt (t) represents the components of the optimized robot's trajectory on the x and y axes respectively; Set obstacle avoidance constraints, including robot obstacle avoidance stability constraints, robot driving speed constraints, and robot driving acceleration constraints: Let the position set of obstacles on the two-dimensional plane be Among them, h represents the obstacle index, e represents the number of obstacles, (x h ,y h ) represents the hth obstacle position; Optimized robot driving trajectory T opt (t) Satisfy the condition of not intersecting with all obstacles, that is, In the formula, t0, t f They represent the start time and end time of the robot's driving trajectory, δ represents the safety distance, and it is required that the distance between the robot's driving trajectory and the obstacle at any time is greater than δ; Let the maximum limit of the robot's speed be v max , then the optimized robot driving trajectory T opt (t)Satisfy: In the formula, Respectively represent the velocity components of the optimized robot's driving trajectory on the x-axis and y-axis; Let the maximum limit of the robot's driving acceleration be a max , then the optimized robot driving trajectory T opt (t)Satisfy: In the formula, Respectively represent the acceleration components of the optimized robot's driving trajectory on the x-axis and y-axis; Get the obstacle set o and check the optimized robot trajectory T opt (t) Whether there is a collision with an obstacle: If a collision occurs, the robot's driving trajectory is replanned.

6. The robot safety trajectory planning and analysis method considering obstacle avoidance stability according to claim 1, characterized in that: In step 4, a control instruction is generated according to the adjusted robot driving trajectory, the control instruction is sent to the robot for execution, and the robot replans the driving trajectory according to the control instruction and completes the obstacle avoidance operation. The process includes: Get the adjusted robot driving trajectory T adj (t), that is, T adj (t) = (x adj (t),y adj (t)), where T adj (t) is the adjusted robot trajectory function, which changes with time t, x adj (t), y adj (t) represents the components of the robot’s trajectory after adjustment on the x and y axes respectively; According to the adjusted robot driving trajectory T adj (t) Generate control instructions: By calculating the adjusted robot trajectory T adj The derivative of (t) obtains the desired velocity and acceleration information: In the formula, are the velocity and acceleration vectors of the robot's trajectory, representing the robot's velocity and acceleration at time t, i.e., the expected velocity and acceleration, Respectively represent the velocity components of the robot's trajectory on the x-axis and y-axis after adjustment, Respectively represent the acceleration components of the robot's trajectory after adjustment on the x-axis and y-axis; Sending the generated control instructions to the robot for execution; wherein the control instructions include the desired speed and acceleration information; The robot plans its driving trajectory according to the control instructions it receives, including adjusting speed, steering angle, etc. During the movement, the robot will constantly monitor the changes in the surrounding environment and the execution of the trajectory.

Images