Robot real-time optimal trajectory planning method based on double-layer data structure

Abstract

This invention discloses a real-time optimal trajectory planning method for robots based on a two-layer data structure, comprising: acquiring information about the robot's surrounding environment; extracting obstacle edge feature points and converting them into a set of closed polygons; connecting visible edges to form a local visible view; directly eliminating visible edges of dynamic obstacles and reconnecting them before merging them into the global layer, and finding the shortest path from the starting point to the ending point; simultaneously decomposing the horizontal and vertical motion trajectories and modeling the trajectories; converting the obtained shortest path into a time-parameterized trajectory; using a cost function to select appropriate control points and calculate the cost of each trajectory to obtain a locally optimal trajectory, which is then used as the local layer; verifying the rationality of the locally optimal trajectory; and obtaining the globally optimal trajectory through the global layer. This invention enables mobile robots to find the optimal driving trajectory in unknown environments, and has the advantages of good real-time performance, low computational cost, and high operating efficiency.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot technology, and relates to robot path planning and trajectory optimization technology, specifically to a robot real-time optimal trajectory planning method based on a two-layer data structure. Background Technology

[0002] Since the beginning of the 21st century, mobile robot technology has gradually risen to the forefront of science and technology. Mobile robots can automate logistics tasks, from indoor transportation tasks (such as automated warehouses, hospitals, homes, and shopping malls) to outdoor transportation tasks (such as parcel delivery). Current trajectory planning methods have several drawbacks: First, they cannot perform trajectory planning in unknown environments, and the algorithm's computational efficiency is low and its real-time performance is poor in large-scale environments. Second, the efficiency of current mobile robots in avoiding dynamic obstacles is still not high, and the trajectory is not smooth enough. Generally speaking, under no constraints, there are countless trajectories for a mobile robot from its starting point to its target point. Based on whether an optimization method is used for optimal trajectory planning, trajectory planning can be divided into general trajectory planning and optimal trajectory planning. This classification method, starting from the requirements of trajectory planning, comprehensively describes the methods and characteristics of trajectory planning, and optimal trajectory planning is currently a research hotspot in trajectory planning.

[0003] Optimal trajectory planning is essential for robots to smoothly and quickly avoid obstacles and reach their target locations during task execution.

[0004] Therefore, a new technical solution is needed to achieve the above objectives. Summary of the Invention

[0005] Purpose of the invention: In order to overcome the shortcomings of the existing technology, a real-time optimal trajectory planning method for robots based on a two-layer data structure is provided. This method enables mobile robots to find the optimal driving trajectory in unknown environments. The decision-making approach is consistent with the actual operating characteristics of mobile robots and has the advantages of good real-time performance, low computational cost, and high operating efficiency.

[0006] Technical Solution: To achieve the above objectives, this invention provides a real-time optimal trajectory planning method for robots based on a two-layer data structure, comprising the following steps:

[0007] S1: Obtain information about the robot's surrounding environment, extract feature points of obstacle edges and convert them into a set of closed polygons, and connect the visible edges to form a local view.

[0008] S2: Determine whether the obstacle is dynamic or static. For dynamic obstacles, directly eliminate the visible edges and reconnect the visible edges before merging them into the global layer. For static obstacles, directly add them to the global layer and find the shortest path from the start point to the end point.

[0009] S3: Simultaneously decompose the horizontal and vertical motion trajectories and model the trajectories. The shortest path obtained in step S2 is transformed into a time-parameterized trajectory. The cost function is used to select appropriate control points and calculate the cost of each trajectory to obtain the local optimal trajectory, which is then used as the local layer.

[0010] S4: Use kinematic constraints and robot driving safety indicators to verify the rationality of the local optimal trajectory, input the local optimal trajectory that meets the rationality requirements into the global layer, and obtain the global optimal trajectory through the global layer.

[0011] The local layer and global layer in this invention constitute a two-layer data structure. The local layer contains local visible views and local optimal paths, while the global layer stores the visualization of the mobile robot's movement process and historical trajectories, thereby completing real-time optimal trajectory planning.

[0012] Furthermore, the construction of the local visible view in step S1 includes the following process:

[0013] A1: The point cloud data S collected by the lidar is downsampled using a point cloud filtering algorithm to obtain a sparse point cloud S';

[0014] A2: Project the sparse point cloud onto the S' plane to obtain a binary image. origin and the binary image Image origin Image generated using mean filtering blur ;

[0015] A3: Image blur blur The contours of obstacles are extracted using a tracking algorithm to obtain their contour feature points. And filter the outline feature points of the obstacles;

[0016] A4: Construct a local visibility view G using the contour feature points of the filtered obstacles. local .

[0017] Furthermore, step S2 specifically includes the following process:

[0018] B1: Move the local view G local Visibility edge set ε in local The edges that enter the obstacle polygon are filtered out, and the set of edges that bypass the obstacle is retained;

[0019] B2: Construct L at the local layer local Constructing a local view G local Then, the local layer L local Vertices and global layer L in glibalVertices in L are associated only if the Euclidean distance between their nearest vertices is less than one-tenth of the distance between their two longest vertices, i.e., the update is related to L. local Overlapping L global And complete the merger;

[0020] B3: Given robot position P robot and target location O goal , will O robot and P goal Add as two vertices to the global layer L global Up, and connect them Vertices with collision-free visible edges;

[0021] B4: If a collision-free path exists, search at the global layer L using a breadth-first search algorithm. global Find P robot and P goal The shortest path between them.

[0022] Furthermore, step S3 specifically includes the following process:

[0023] C1: Set the shortest path obtained in step S2 as the reference path, convert the reference path into a time-parameterized trajectory, and sample the trajectory at fixed time intervals Δt to obtain a set of control points. Then, optimize the control points.

[0024] C2: For longitudinal and lateral displacements s i (t), d i (t) Select cubic polynomials as trajectory functions respectively, and obtain the longitudinal and transverse displacement parameters according to the cubic spline curve solution method;

[0025] C3: Combine longitudinal and lateral displacements to establish a cost function to evaluate the quality of the trajectory.

[0026] Furthermore, step S4 specifically includes the following process:

[0027] D1: The robot motion is modeled using deterministic discrete-time nonlinear dynamics, expressed by the following formula:

[0028] x k+1 =f(x) k ,u k (1)

[0029] in and These are the state and input of discrete-time instance k, respectively, and n x and n u These are the state and input dimensions, respectively. The state includes the robot's two-dimensional position. Where R 2Represents a two-dimensional space vector. Indicates a dimension of n x Spatial vector;

[0030] D2: The position of obstacle j at time k = 0 is represented as... For each obstacle, its time steps over the next N time steps (i.e. Motion prediction can be used for robot instances at every time step;

[0031] D3: Model the obstacle and robot regions using a disk with radii r and r respectively. obs And r, and then judge whether the optimal trajectory selected in step S3 meets the driving safety and dynamic constraints, so that the mobile robot can drive safely and without collision. If it meets the requirements, the optimal trajectory is output to the local layer. Then, the trajectory in the local layer during the movement of the mobile robot is added to the global layer. The real-time optimal planning trajectory is obtained by constructing a two-layer data structure consisting of the local layer and the global layer.

[0032] Further, step C1 specifically includes:

[0033] C1-1: By planning collision-free paths in the state space, the state space, consisting of the workspace and time, is defined by X = R. 2 ×[0,T] describes the state-space path as a continuous domain, where [0,T] represents time, and τ represents time parameterization. geometric :[0,T]→R 2 Each trajectory is sampled at a fixed time interval Δt to obtain a set of control points Q = [Q0,...,Q N ], where Q i ∈R 2 This invention optimizes these control points to improve the smoothness of position and velocity;

[0034] C1-2: Use J points The cost function for the sampled control points is expressed by the following formula:

[0035] J points =J geo +J smooth +J obst +J vel (2)

[0036] C1-3: Geometric cost function J geo Represents the distance from the geometric trajectory to the original control point. The distance between them is expressed by the following formula:

[0037]

[0038] C1-4: Smoothness Cost Function J smooth The formula is expressed as follows:

[0039]

[0040] C1-5: Barrier Cost Function J obst The aim is to exponentially penalize the straight-line distance from control point i to obstacle j. make Where time index k and Q i The associated time matching, hence the obstacle cost function J obst The formula is expressed as follows:

[0041]

[0042] in

[0043] C1-6: Speed ​​Cost Function J vel This refers to the speed relative to the tracking speed V. ref The offset, the control point velocity V of the geometric path i The announcement is as follows:

[0044]

[0045] The relevant speed optimization path formula is then expressed as follows:

[0046]

[0047] in This indicates the robot's current speed. and Let i and i+1 represent the robot's speeds, respectively.

[0048] C1-7: The cost is calculated by constructing a trajectory that satisfies the velocity tracking cost based on the geometric trajectory, and then penalizing the distance to that trajectory; the velocity cost function J vel Its formula is expressed as follows:

[0049]

[0050] C1-8: By calculating J in step C1-2 points Select the control point with the lowest cost and complete the optimization of the control point.

[0051] Further, step C2 specifically includes:

[0052] C2-1: Modeling longitudinal and lateral displacements within the target area, expressed by the following formulas:

[0053]

[0054] Where s i (t) and d i (t) represent the longitudinal and lateral displacement functions of the mobile robot, respectively. b0 represents the longitudinal displacement coefficient. Indicates the lateral displacement coefficient;

[0055] C2-2: Based on the four properties of cubic spline sampling: curves must be smooth at knots, curves must have equal slopes at knots, curves must have equal curvatures at knots, and the conditions for the start and end points of the curves, all unknowns can be obtained, thus obtaining all cubic spline curves.

[0056] C2-3: By optimizing the control points, cubic splines are fitted to the x and y positions respectively to obtain a continuous trajectory, which is then formed by segment τ. i (t)=[s i (t)d i (t)] T Composition, where s i (t) and d i (t) is derived from τ i (t) yields τ i (t) It is a twice continuously differentiable cubic spline curve formed by optimized control points, which forms a smooth trajectory τ:[0,T]→R from the current robot position to the target. 2 .

[0057] Furthermore, step C3 specifically includes:

[0058] C3-1: The trajectory of a mobile robot should meet the requirement of minimizing the path length, which is achieved by sampling the position p at constant time intervals. i The relative positions of the two positions are used for evaluation; the smaller the absolute difference between the two positions, the shorter the path. Let J1 represent the cost function for the length of the mobile robot's travel path, and its formula is as follows:

[0059] J1=||p i -p i-1 || (10)

[0060] C3-2: The mobile robot prioritizes the trajectory with the smallest speed difference, penalizing trajectories that are too fast or too slow. The formula for calculating the trajectory speed cost function value J2 is as follows:

[0061]

[0062] Where v i To sample at a constant time interval, This is a reference speed;

[0063] C3-3: The smoothness of mobile robot movement is evaluated using acceleration. Generally, the smaller the acceleration in both directions, the smoother the movement. J3 represents the cost function for smooth mobile robot movement, and its formula is as follows:

[0064] J3=α i ||a i || (12)

[0065] Where α≈1 is the acceleration later in the discount trajectory, a i It is acceleration;

[0066] C3-4: Mobile robots should complete the obstacle avoidance process as quickly as possible to reduce the impact on overall traffic efficiency. The overall obstacle avoidance time is represented by T1, and the cost function value J2 of trajectory efficiency is calculated using the following formula:

[0067]

[0068] Where T max This represents the maximum obstacle avoidance time across all trajectories.

[0069] C3-5: Considering the performance metrics of steps C3-1, C3-2, C3-3, and C3-4, a cost function is proposed to evaluate the optimal obstacle avoidance trajectory function as follows:

[0070]

[0071] Where J represents the cost function, I is the number of samples, and ω L ω V ω a ω c ω t The weight is represented by C, which is a constant penalty if the trajectory was not selected in the previous iteration, thus selecting the trajectory with the lowest cost.

[0072] This invention provides a real-time optimal trajectory method based on a two-layer data structure. In both known and unknown environments, it can quickly avoid obstacles and follow the optimal trajectory, significantly reducing computational load and improving real-time performance. Based on a visualization graph, this invention uses the area currently observed by the mobile robot as the local layer. Within the local layer, the horizontal and vertical motion trajectories are decomposed and modeled. A cost function is used to select control points and calculate the cost of each trajectory to obtain the locally optimal trajectory, which is then transmitted to the global layer. The global layer stores all historical messages from the local layer. The local and global layers together constitute a two-layer data structure. Trajectory planning based on this structure can plan trajectories with optimal time and stability. This is of great significance for robots to quickly avoid obstacles and improve the working efficiency of mobile robots.

[0073] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0074] (1) This invention provides a method that, based on a visualization graph, uses the range currently observed by the mobile robot as a local layer, while a global layer stores all historical messages from the local layer. The local and global layers together constitute a two-layer data structure. Trajectory planning based on this two-layer data structure can greatly reduce computation and improve real-time performance.

[0075] (2) The present invention provides a method to obtain the local optimal trajectory by decomposing the horizontal and vertical motion trajectories in the local layer and modeling the trajectory, using the cost function to select control points and calculate the cost of each trajectory, and then sending it to the global layer.

[0076] (3) This invention provides a method for modeling obstacles and robot regions using a disk with radii r and r, respectively. obs And r, and then determine whether the selected optimal trajectory plan meets driving safety and dynamic constraints, so that the mobile robot can drive safely and without collision.

[0077] (4) The method of the present invention has the advantages of good real-time performance, low-cost computation and high operating efficiency. Attached Figure Description

[0078] Figure 1 This is a flowchart of the method of the present invention;

[0079] Figure 2 This is a schematic diagram of the process of constructing a local visible view in this invention;

[0080] Figure 3 This is a diagram showing the range of the local layer and the global layer in a specific embodiment of the present invention;

[0081] Figure 4 This is a time-lapse graph showing the update time of the mobile robot visualization in a specific embodiment of the present invention;

[0082] Figure 5 This is a schematic diagram of mobile robot trajectory optimization in a specific embodiment of the present invention;

[0083] Figure 6 This is a diagram illustrating the geometric path and control points of the mobile robot in a specific embodiment of the present invention.

[0084] Figure 7 This is a diagram showing the trajectory planning effect before optimization in a specific embodiment of the present invention;

[0085] Figure 8 This is a diagram showing the optimized trajectory planning effect in a specific embodiment of the present invention. Detailed Implementation

[0086] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. After reading this invention, any modifications of the invention in various equivalent forms by those skilled in the art will fall within the scope defined by the appended claims.

[0087] This invention provides a real-time optimal trajectory planning method for robots based on a two-layer data structure, such as... Figure 1 As shown, it includes the following steps:

[0088] S1: Obtain information about the robot's surrounding environment through LiDAR, use image processing technology to extract obstacle edge feature points and convert them into a set of closed polygons, and connect the visible edges to form a local view.

[0089] S2: Determine whether the obstacle is dynamic or static. For dynamic obstacles, directly eliminate the visible edges and reconnect the visible edges before merging them into the global layer. For static obstacles, directly add them to the global layer and find the shortest path from the start point to the end point.

[0090] S3: Simultaneously decompose the horizontal and vertical motion trajectories and model the trajectories. The shortest path obtained in step S2 is transformed into a time-parameterized trajectory. The cost function is used to select appropriate control points and calculate the cost of each trajectory to obtain the local optimal trajectory, which is then used as the local layer.

[0091] S4: If the path planning is successful, the rationality of the local optimal trajectory is verified by kinematic constraints and robot driving safety indicators. The local optimal trajectory that meets the rationality requirements is input into the global layer, and the global optimal trajectory is obtained through the global layer.

[0092] The local layer and the global layer constitute a two-layer data structure. The local layer contains local visible views and local optimal paths, while the global layer stores the visualization of the mobile robot's movement process and historical trajectories, thereby completing real-time optimal trajectory planning.

[0093] In this embodiment, step S1 provides a method for constructing a local visible view, such as... Figure 2 As shown, the specific construction process is as follows:

[0094] A1: The point cloud data S collected by the lidar is downsampled using a point cloud filtering algorithm to obtain a sparse point cloud S';

[0095] A2: Project the sparse point cloud onto the S' plane to obtain a binary image. origin and the binary image Image origin Image generated using mean filtering blur ;

[0096] A3: Image blur blur The contours of obstacles are extracted using a tracking algorithm to obtain their contour feature points. And filter the outline feature points of the obstacles;

[0097] A4: Construct a local visibility view G using the contour feature points of the filtered obstacles. local .

[0098] Reference Figure 3 The diagram shows the local and global layer extents, with red and green squares representing the local layer extents and cyan-green areas representing the global layer extents. In this embodiment, step S2 specifically includes the following process:

[0099] B1: Move the local view G local Visibility edge set ε in local The edges that enter the obstacle polygon are filtered out, and the set of edges that bypass the obstacle is retained;

[0100] B2: Construct L at the local layer local Constructing a local view G local Then, the local layer L local Vertices and global layer L in global Vertices in L are associated only if the Euclidean distance between their nearest vertices is less than one-tenth of the distance between their two longest vertices, i.e., the update is related to L. local Overlapping L global And complete the merger;

[0101] B3: Given robot position P robot and target location P goal , will P robot and P goal Add as two vertices to the global layer L global Up, and connect them Vertices with collision-free visible edges;

[0102] B4: If a collision-free path exists, search at the global layer L using a breadth-first search algorithm. global Find P robot and P goal The shortest path between them.

[0103] Step S3 specifically includes the following process:

[0104] C1: Set the shortest path obtained in step S2 as the reference path, convert the reference path into a time-parameterized trajectory, and sample the trajectory at fixed time intervals Δt to obtain a set of control points. Then, optimize the control points:

[0105] Step C1 is as follows:

[0106] C1-1: By planning collision-free paths in the state space, the state space, consisting of the workspace and time, is defined by X = R. 2 ×[0,T] describes the state-space path as a continuous domain, where [0,T] represents time, and τ represents time parameterization. geometric :[0,T]→R 2 Each trajectory is sampled at a fixed time interval Δt to obtain a set of control points Q = [Q0,...,Q N ], where Q i ∈R 2 This invention optimizes these control points to improve the smoothness of position and velocity;

[0107] C1-2: Use J points The cost function for the sampled control points is expressed by the following formula:

[0108] J points =J geo +J smooth +J obst +J vel (2)

[0109] C1-3: Geometric cost function J geo Represents the distance from the geometric trajectory to the original control point. The distance between them is expressed by the following formula:

[0110]

[0111] C1-4: Smoothness Cost Function J smooth The formula is expressed as follows:

[0112]

[0113] C1-5: Barrier Cost Function J obst The aim is to exponentially penalize the straight-line distance from control point i to obstacle j. make Where time index k and Q i The associated time matching, hence the obstacle cost function J obst The formula is expressed as follows:

[0114]

[0115] in

[0116] C1-6: Speed ​​Cost Function J vel This refers to the speed relative to the tracking speed V. ref The offset, the control point velocity V of the geometric path i The announcement is as follows:

[0117]

[0118] The relevant speed optimization path formula is then expressed as follows:

[0119]

[0120] in This indicates the robot's current speed. and Let i and i+1 represent the robot's speeds, respectively.

[0121] C1-7: The cost is calculated by constructing a trajectory that satisfies the velocity tracking cost based on the geometric trajectory, and then penalizing the distance to that trajectory; the velocity cost function J vel Its formula is expressed as follows:

[0122]

[0123] C1-8: By calculating J in step C1-2 points Select the control point with the lowest cost and complete the optimization of the control point.

[0124] C2: For longitudinal and lateral displacements s i (t), d i (t) Selecting cubic polynomials as trajectory functions, the longitudinal and lateral displacement parameters are obtained using the cubic spline curve solution method:

[0125] Step C2 is as follows:

[0126] C2-1: Modeling longitudinal and lateral displacements within the target area, expressed by the following formulas:

[0127]

[0128] Where s i (t) and d i (t) represent the longitudinal and lateral displacement functions of the mobile robot, respectively. b0 represents the longitudinal displacement coefficient. Indicates the lateral displacement coefficient;

[0129] C2-2: Based on the four properties of cubic spline sampling: curves must be smooth at knots, curves must have equal slopes at knots, curves must have equal curvatures at knots, and the conditions for the start and end points of the curves, all unknowns can be obtained, thus obtaining all cubic spline curves.

[0130] C2-3: By optimizing the control points, cubic splines are fitted to the x and y positions respectively to obtain a continuous trajectory, which is then formed by segment τ.i (t)=[s i (t)d i (t)] T Composition, where s i (t) and d i (t) is derived from τ i (t) yields τ i (t) It is a twice continuously differentiable cubic spline curve formed by optimized control points, which forms a smooth trajectory τ:[0,T]→R from the current robot position to the target. 2 .

[0131] C3: Evaluate the quality of a trajectory by establishing a cost function that combines longitudinal and lateral displacements.

[0132] Step C3 is as follows:

[0133] C3-1: The trajectory of a mobile robot should meet the requirement of minimizing the path length, which is achieved by sampling the position p at constant time intervals. i The relative positions of the two positions are used for evaluation; the smaller the absolute difference between the two positions, the shorter the path. Let J1 represent the cost function for the length of the mobile robot's travel path, and its formula is as follows:

[0134] J1=||p i -p i-1 || (10)

[0135] C3-2: The mobile robot prioritizes the trajectory with the smallest speed difference, penalizing trajectories that are too fast or too slow. The formula for calculating the trajectory speed cost function value J2 is as follows:

[0136]

[0137] Where v i To sample at a constant time interval, This is a reference speed;

[0138] C3-3: The smoothness of mobile robot movement is evaluated using acceleration. Generally, the smaller the acceleration in both directions, the smoother the movement. J3 represents the cost function for smooth mobile robot movement, and its formula is as follows:

[0139] J3=α i ||a i || (12)

[0140] Where α≈1 is the acceleration later in the discount trajectory, a i It is acceleration;

[0141] C3-4: Mobile robots should complete the obstacle avoidance process as quickly as possible to reduce the impact on overall traffic efficiency. The overall obstacle avoidance time is represented by T1, and the cost function value J2 of trajectory efficiency is calculated using the following formula:

[0142]

[0143] Where T max This represents the maximum obstacle avoidance time across all trajectories.

[0144] C3-5: Considering the performance metrics of steps C3-1, C3-2, C3-3, and C3-4, a cost function is proposed to evaluate the optimal obstacle avoidance trajectory function as follows:

[0145]

[0146] Where J represents the cost function, I is the number of samples, and ω L ω V ω a ω c ω t The weight is represented by C, which is a constant penalty if the trajectory was not selected in the previous iteration, thus selecting the trajectory with the lowest cost.

[0147] Step S4 specifically includes the following process:

[0148] D1: The robot motion is modeled using deterministic discrete-time nonlinear dynamics, expressed by the following formula:

[0149] x k+1 =f(x) k ,u k (1)

[0150] in and These are the state and input of discrete-time instance k, respectively, and n x and n u These are the state and input dimensions, respectively. The state includes the robot's two-dimensional position. Where R 2 Represents a two-dimensional space vector. Indicates a dimension of n x Spatial vector;

[0151] D2: The position of obstacle j at time k = 0 is represented as... For each obstacle, its time steps over the next N time steps (i.e. Motion prediction can be used for robot instances at every time step;

[0152] D3: Model the obstacle and robot regions using a disk with radii r and r respectively. obs And r, and then judge whether the optimal trajectory selected in step S3 meets the driving safety and dynamic constraints, so that the mobile robot can drive safely and without collision. If it meets the requirements, the optimal trajectory is output to the local layer. Then, the trajectory in the local layer during the movement of the mobile robot is added to the global layer. The real-time optimal planning trajectory is obtained by constructing a two-layer data structure consisting of the local layer and the global layer.

[0153] This embodiment applies the above solution to an example, and obtains... Figures 4 to 8 The data and effect diagrams are analyzed in detail below:

[0154] Figure 4 This refers to the time taken to update the mobile robot visualization. Figure 4 The third line of Chinese text indicates that the total number of corner points in the local layer is 116, and the fifth line indicates that the time taken to update the visualization is 74.547ms. Figure 4 The last line of Chinese text indicates that the total number of corner points in the global layer is 210. The time taken of 74.547ms demonstrates the fast computation speed of the mobile robot based on a two-layer data structure for real-time trajectory optimization.

[0155] Figure 5 This is a schematic diagram of a mobile robot's trajectory optimization. The black part represents obstacles, the blue line represents the reference trajectory, and the red line represents the motion trajectory after real-time optimization using a two-layer data structure. Figure 6 This is a diagram of the geometric path and control points of a mobile robot. The green dot is the starting point and the red dot is the ending point. The mobile robot records obstacles in the environment (red polygons) and constructs a visualization (cyan-green line). The endpoints of the cyan-green line are the optimized control points. Figure 7 This is a diagram showing the trajectory planning effect of a mobile robot before optimization. Figure 7 The colored lines represent the robot's trajectory from the green starting point to the red ending point; Figure 8 This is a diagram showing the optimized trajectory planning effect of a mobile robot. Figure 8 The colored lines represent the robot's trajectory from the green starting point to the red ending point, showing that the mobile robot's trajectory has become smoother with fewer large curvature bends, indicating that the trajectory optimization is effective.

[0156] This embodiment also provides a robot real-time optimal trajectory planning system based on a two-layer data structure. The system includes a network interface, a memory, and a processor. The network interface is used to receive and send signals during the process of sending and receiving information with other external network elements. The memory is used to store computer program instructions that can run on the processor. The processor is used to execute the steps of the consensus method described above when running the computer program instructions.

[0157] This embodiment also provides a computer storage medium storing a computer program that, when executed by a processor, can implement the methods described above. The computer-readable medium can be considered tangible and non-transitory. Non-limiting examples of non-transitory tangible computer-readable media include non-volatile memory circuitry (e.g., flash memory circuitry, erasable programmable read-only memory circuitry, or masked read-only memory circuitry), volatile memory circuitry (e.g., static random access memory circuitry or dynamic random access memory circuitry), magnetic storage media (e.g., analog or digital magnetic tape or hard disk drive), and optical storage media (e.g., CD, DVD, or Blu-ray disc). The computer program includes processor-executable instructions stored on at least one non-transitory tangible computer-readable medium. The computer program may also include or depend on stored data. The computer program may include a basic input / output system (BIOS) for interacting with the hardware of a dedicated computer, device drivers for interacting with specific devices of the dedicated computer, one or more operating systems, user applications, background services, background applications, etc.

[0158] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied 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.

[0159] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0160] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxesFigure 1 The function specified in one or more boxes.

[0161] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

Claims

1. A method for real-time optimal trajectory planning of a robot based on a two-layer data structure, characterized in that, Includes the following steps: S1: Acquire information about the robot's surrounding environment, extract feature points of obstacle edges and convert them into a set of closed polygons, and connect the visible edges to form a local view. S2: Determine whether the obstacle is dynamic or static. For dynamic obstacles, directly eliminate the visible edges and reconnect the visible edges before merging them into the global layer. For static obstacles, directly add them to the global layer and find the shortest path from the start point to the end point. S3: Simultaneously decompose the horizontal and vertical motion trajectories and model the trajectories. Transform the shortest path obtained in step S2 into a time-parameterized trajectory. Use the cost function to select control points and calculate the cost of each trajectory to obtain the local optimal trajectory, and use this as the local layer. S4: Use kinematic constraints and robot driving safety indicators to verify the rationality of the local optimal trajectory, input the local optimal trajectory that meets the rationality requirements into the global layer, and obtain the global optimal trajectory through the global layer; Step S3 specifically includes the following process: C1: Set the shortest path obtained in step S2 as the reference path, convert the reference path into a time-parameterized trajectory, and apply the trajectory at fixed time intervals. A set of control points is obtained through sampling, and then the control points are optimized. C2: For longitudinal and lateral displacement , A cubic polynomial was selected as the trajectory function, and the longitudinal and lateral displacement parameters were obtained according to the solution method of cubic spline curves. C3: Combine longitudinal and lateral displacements to establish a cost function to evaluate the quality of the trajectory; Step S4 specifically includes the following process: D1: The robot motion is modeled using deterministic discrete-time nonlinear dynamics, expressed by the following formula: ； in ∈ and ∈ These are the state and input of the discrete-time instance k, respectively. and These are the state and input dimensions, respectively. The state includes the robot's two-dimensional position. ,in Represents a two-dimensional space vector. The dimension is Spatial vector; D2: The position of obstacle j at time k=0 is represented as... ∈ For each obstacle, its motion prediction over the next N time steps can be used for the robot's instance at each time step, where N is... ,..., ; D3: Model the obstacle and robot regions using a disk, with radii of 1 and 2 respectively. And r, and then judge whether the optimal trajectory selected in step S3 meets the driving safety and dynamic constraints, so that the mobile robot can drive safely and without collision. If it meets the requirements, the optimal trajectory is output to the local layer. Then, the trajectory in the local layer during the movement of the mobile robot is added to the global layer. The real-time optimal planning trajectory is obtained by constructing a two-layer data structure consisting of the local layer and the global layer.

2. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 1, characterized in that, The construction of the local visible view in step S1 includes the following process: A1: The point cloud data S collected by the lidar is downsampled using a point cloud filtering algorithm to obtain a sparse point cloud. ; A2: Sparse point cloud Binary image obtained by planar projection and the binary image Blurred image generated using mean filtering ; A3: Blur image The contours of obstacles are extracted using a tracking algorithm to obtain their contour feature points. And filter the outline feature points of the obstacles; A4: Construct a local visibility view using the contour feature points of the filtered obstacles. .

3. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 2, characterized in that, Step S2 specifically includes the following process: B1: View the partial visibility Visibility edge set in The edges that enter the obstacle polygon are filtered out, and the set of edges that bypass the obstacle is retained; B2: Build at a local layer Build a local view Then, the local layer Vertices and global layer The vertices in the array are associated only if the Euclidean distance between their nearest vertices is less than one-tenth of the distance between their longest vertices, i.e., the update is related to... Overlapping And complete the merger; B3: Given robot position and target location ,Will and Add as two vertices to the global layer Up, and connect them Vertices with collision-free visible edges; B4: If a collision-free path exists, search the global layer using a breadth-first search algorithm. turn up and The shortest path between them.

4. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 1, characterized in that, Step C1 specifically involves: C1-1: By planning collision-free paths in the state space, the state space, consisting of the workspace and time, is defined by X = The [0,T] description, where [0,T] is a continuous domain and T represents time, transforms the state-space path into a time-parameterized trajectory. :[0,T]→ Each trajectory is at a fixed time interval Sampling yields a set of control points Q=[ ,..., ],in ∈ ; C1-2: Use The cost function for the sampled control points is expressed by the following formula: ； C1-3: Geometric Cost Function Represents the distance from the geometric trajectory to the original control point. The distance between them is expressed by the following formula: ； C1-4: Smoothness Cost Function The formula is expressed as follows: ； C1-5: Barrier Cost Function The aim is to use exponentially penalized control points To the obstacle straight-line distance ,make Where time index k and The associated time matching, hence the barrier cost function The formula is expressed as follows: ； in , ; C1-6: Speed ​​Cost Function This refers to the speed relative to tracking speed. The offset, the control point velocity of the geometric path The formula is expressed as follows: ； The relevant speed optimization path formula is then expressed as follows: ； in This indicates the robot's current speed. and They represent and The robot's speed at all times; C1-7: The cost is calculated by constructing a trajectory that satisfies the velocity tracking cost based on the geometric trajectory, and then penalizing the distance to that trajectory; velocity cost function. Its formula is expressed as follows: ； C1-8: By calculating step C1-2 Select the control point with the lowest cost and complete the optimization of the control point.

5. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 1, characterized in that, Step C2 specifically involves: C2-1: Modeling longitudinal and lateral displacements within the target area, expressed by the following formulas: ； in and Let these represent the longitudinal and lateral displacement functions of the mobile robot, respectively. , ... Indicates the longitudinal displacement coefficient. , ... Indicates the lateral displacement coefficient; C2-2: Based on the four properties of cubic spline sampling, all unknowns are obtained, that is, all cubic spline curves are obtained; C2-3: By optimizing the control points, cubic splines are fitted to the x and y positions respectively to obtain a continuous trajectory, which is then formed by segments. = Composition, in which and Depend on get, It is a cubic spline curve that is twice continuously differentiable and, through optimized control points, forms a smooth trajectory τ:[0,T]→ from the current robot position to the target. .

6. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 1, characterized in that, Step C3 specifically involves: C3-1: The trajectory of a mobile robot should meet the requirement of minimizing the path length, which is achieved by sampling the position at constant time intervals. The relative position is used for evaluation; The cost function representing the length of the mobile robot's travel path is expressed by the following formula: ； C3-2: The mobile robot prioritizes the trajectory with the smallest speed difference, penalizing trajectories that are too fast or too slow. The cost function value for trajectory speed is... The calculation formula is as follows: ； in To sample at a constant time interval, This is a reference speed; C3-3: The requirement for the smoothness of mobile robot movement is evaluated using acceleration; The cost function representing the smooth movement of the mobile robot is expressed by the following formula: ； Where α≈1 is the acceleration later in the discount trajectory, It is acceleration; C3-4: Mobile robots should complete obstacle avoidance as quickly as possible to minimize the impact on overall traffic efficiency. The overall obstacle avoidance time should be [specified]. This represents the cost function value of trajectory efficiency. The calculation formula is as follows: ； in The maximum obstacle avoidance time across all trajectories; C3-5: Considering the performance metrics of steps C3-1, C3-2, C3-3, and C3-4, a cost function is proposed to evaluate the optimal obstacle avoidance trajectory function as follows: ； in Let I represent the cost function, where I is the number of samples. , , , , Indicates weight, This means that if the trajectory was not selected in the previous iteration, a constant penalty is applied, and the trajectory with the lowest cost is selected.

7. The method for real-time optimal trajectory planning of a robot based on a two-layer data structure according to claim 5, characterized in that, The four properties of cubic spline sampling in step C2-2 include the requirement that the curve be smooth at the kink, that the curve have equal slopes at the kink, that the curve have equal curvatures at the kink, and the conditions for the start and end points of the curve.

Images