A robot time-energy optimal smooth trajectory planning method

Abstract

A robot time-energy optimal smooth trajectory planning method is provided, an energy function is constructed by the square sum of path curvature rate, a cost function is constructed by the weighted sum of path energy and length, an optimization model is constructed by introducing path curvature peak and safety distance constraints, and the optimal path is searched under the environment of Delaunay triangulation. The initial path is re-optimized through deleting acute angle vertex, path replacement, removing redundant points, etc., is fitted by cubic NURBS, and is controlled by five-section S-shaped acceleration and deceleration motion under the Bang-Bang-Singular trajectory tangential jerk strategy, so that the path is smooth and collision-free, the trajectory satisfies the curvature peak and curvature rate constraints, and the time-energy is optimal. The robot is not simplified as a point, and the working environment and whether the obstacle is regular are not limited, the search of the path of any two-dimensional space containing obstacles can be processed, the smooth trajectory and fast motion are ensured, and the unified planning of geometry and motion is realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of mobile robot motion planning. BACKGROUND

[0002] In the aspect of robot path planning, the commonly used road map method, unit decomposition method and potential field method are relatively classic traditional methods, and the technology is relatively mature. With the development of artificial intelligence technology and the improvement of the autonomy of robots, many scholars focus on developing group intelligence path planning methods such as genetic algorithm, ant colony algorithm, firefly algorithm and machine learning-based path planning methods such as neural network and reinforcement learning.

[0003] Compared with path planning, which only needs to consider the geometric constraints of the robot workspace and find a collision-free feasible route, trajectory planning assigns time information to the path and needs to combine kinematic and dynamic constraints such as displacement, velocity and acceleration, jerk and force or torque during the motion process. Alshaikhli et al. (Alshaikhli O A, Al-Araji A. Path Planning and Control Strategy Design for Mobile Robot Based on Hybrid Swarm Optimization Algorithm[J]. International Journal of Intelligent Engineering and Systems, 2021, 14(3): 2021.) solved the minimum cost trajectory planning method through the control problem and proposed a robot trajectory control strategy based on the optimal control of position and direction parameters. Ni et al. (Ni H, Ji S, Liu Y, et al. Velocity planning method for position-velocity-time control based on a modified S-shaped acceleration / deceleration algorithm[J]. International Journal of Advanced Robotic Systems, 2022, 19(1): 166-175.) proposed a trajectory planning algorithm based on the modified S-shaped velocity profile rule and applied it to the two-axis robot platform to control the kinematic parameters within the allowed range. Li et al. (Li Z N, Wang T, Wang B R, et al. Trajectory planning for manipulator in Cartesian space based on S-shaped velocity curve with constraints[J]. International Journal of Intelligent Systems, 2019, 14(4): 655-661.) designed an S-shaped velocity curve using space straight line and space circular arc interpolation, which realized the automatic adjustment and continuity of velocity and acceleration under the constraint of path length. Fang (Fang Y, Hu J, Liu W, et al. Smooth and time-optimal S-curve trajectory planning for automated robots and machines[J]. Mechanism and Machine Theory, 2019, 137: 127-153.) designed an asymmetric S trajectory to ensure the high-speed start-stop performance of the robot.

[0004] In terms of trajectory planning, many scholars have studied the motion trajectory of robot arms. Pereira et al. (Pereira FL, Chertovskih R, Daryina A, et al. A Regularization Approach to Analyze the Time-Optimal Motion of a Mobile Robot under State Constraints using Pontryagin's Maximum Principle [J]. Procedia Computer Science, 2021, 186 (4): 11-20.) Based on the indirect numerical calculation of Pontryagin's maximum principle, a planning method for the time-optimal motion of a robot under obstacles and other state constraints was proposed. Zhang Huawen (Zhang Huawen, Liu Ziliang. Robot multi-path point trajectory planning based on time optimization [J]. Automation and Instrumentation, 2022, 37 (06): 43-48.) Based on the quadratic parabola acceleration operation mode, displacement, velocity and acceleration were calculated, and a time-optimal multi-path point trajectory planning algorithm that maintains smooth and continuous trajectory was proposed. Cheng Zhenyi (Cheng Zhenyi, Guo Qiang, Yu Haidong. Optimal trajectory planning for robot impact for wheel hub grinding [J]. Agricultural Equipment and Vehicle Engineering, 2020, 58 (07): 71-75.) searched for the minimum impact trajectory by optimizing the combination of cubic and quintic polynomial spline interpolation functions and executed it in the wheel hub grinding robot. Abdulakareem et al. (Abdulakareem MI, Raheem F A. Development of path planning algorithm using probabilistic roadmap based on ant colony optimization [J]. Engineering and Technology Journal, 2020, 38 (3): 343-351.) solved the trajectory smoothness and endpoint controllability planning problem when there are cusps and mutations at a distance from the cubic B-spline interpolation point. Liu (Liu H, Li G, Xiao J.AC 3 Continuous Toolpath Corner Smoothing Method for a Hybrid Machining Robot [J]. Journal of Manufacturing Processes, 2022, 75(3): 1072-1088.) The kinematic constraints are converted into B-spline interpolation point constraints to achieve the TriMule-800 robot trajectory C 3Continuous angle smoothing. Nagy Vajk I. Nonconvex Time-optimal Trajectory Planning for Robot Manipulators[J]. Journal of Dynamic Systems, Measurement, and Control, 2019, 141(11): 111007.) Taking the running time as the goal, a new velocity profile generation method is proposed based on the torque and viscous friction constraints related to speed, and the nonconvex time-optimal trajectory planning of the robot is realized. In terms of energy-optimal trajectory planning, considering that smooth trajectories are easier to track and can reduce the stress on the actuator and manipulator and save energy, Le (Le AV, Ku PC, Than Tun T, et al. Realization Energy Optimization of Complete Path Planning in Differential Drive based Self-reconfigurable FloorCleaning Robot[J]. Energies, 2019, 12(6): 1136.) constructed an energy-aware complete coverage path planning model and used genetic algorithm and ant colony optimization algorithm to obtain the energy-optimal trajectory of all waypoints in the connected workspace. Li Chunyan (Li Chunyan, Chao Yongsheng, Chen Shuai, et al. Energy-optimal trajectory planning for robots based on an improved sparrow search algorithm [J]. Combined Machine Tools and Automated Machining Technology, 2022(06):180-182+187.) used 7th-order B-spline to construct joint space trajectories, combined kinematic parameters with dynamic parameters to calculate the total energy consumption of the robot, and obtained the energy-optimal trajectory based on the improved sparrow search algorithm. The above algorithms have all contributed to optimal trajectory planning, but they do not take into account the fact that mobile robots cannot move continuously at turning points due to their own non-holonomic constraints. It is necessary to strictly limit the peak curvature and curvature change rate of the trajectory. They also do not comprehensively consider issues such as path length, width, and turning angle to process the path. This makes it difficult to effectively solve the various practical problems in mobile robot trajectory planning. Summary of the Invention

[0005] The purpose of the invention is to overcome the shortcomings of the existing technology and provide a time-energy optimal smooth trajectory planning method based on cubic NURBS to solve the time-energy-smoothing problem of the mobile robot path that has non-complete constraints and cannot move continuously at the turning point, the problem that the zero-width line path cannot safely avoid obstacles, and the five-segment S-shaped acceleration and deceleration control problem of trajectory motion.

[0006] The application is realized by the following technical solutions.

[0007] The application constructs an energy function by the square sum of the path curvature change rate, constructs a cost function by the weighted sum of the energy function and the length index value, introduces a path curvature peak value and a safety distance constraint to build an optimization model, and proposes a time-energy optimal smooth path search method without collision under the Delaunay triangulation environment. The path obtained through the re-optimization operations such as deleting the acute angle vertex, path replacement, removing redundant points and NURBS fitting processing not only meets the requirements of the mobile robot on the trajectory curvature peak value and the curvature change rate limit, realizes the optimal goal of time-energy, but also considers the safety distance, and improves the defects that the conventional algorithm idealizes the path as a line path with zero width and cannot safely avoid obstacles. In order to further improve the stability and efficiency of the robot work, a five-segment S-shaped acceleration and deceleration motion control method is also proposed, which satisfies the Bang-Bang-Singular strategy of the trajectory tangent jerk. Simulation and experimental verification show that the method can process the search of the two-dimensional space path containing obstacles, ensure the smoothness of the trajectory curve and the dynamics and flexibility of the system during motion, and realize the unified planning of geometry and motion.

[0008] The application is a new method of time-energy optimal smooth trajectory planning based on cubic NURBS, which is constructed for the optimal search and smoothing processing of the mobile robot path, safe obstacle avoidance, and accurate control of the motion trajectory. The method can solve the problem that the existing robot path planning weakens the motion smoothness and energy consumption performance due to the neglect of the curvature peak value and the curvature change rate limit of the path, and at the same time, the trajectory is smoothed, the time-energy is optimal, and the safety obstacle avoidance is realized by setting the path width.

[0009] The robot time-energy optimal smooth trajectory planning method provided by the application comprises the following steps:

[0010] Step S1, map environment construction based on Delaunay triangulation. It includes determining the coordinate values of the edges and vertices of the obstacles in the task space, removing the triangulation triangles inside the obstacles based on the Delaunay triangulation, and including the boundaries of the obstacles to form a new triangulation network, etc. It contains the standards for (a) corner method to judge whether a point is outside a polygon obstacle, and (b) to judge whether the whole or part of a triangle in the triangulation network is inside an obstacle. The midpoint of the free edge of the triangulation network in the map is defined as the optional path point for subsequent path search, and the edge set of the optional path is the set of the midpoints of the two free edges of the adjacent triangles, and the set of the midpoints of the adjacent free edges of the starting point and the ending point.

[0011] Step S2, initial fold line path planning. Considering that the mobile robot has nonholonomic constraints, it cannot continuously move at the turning point, it is necessary to strictly limit the trajectory curvature peak value and the curvature rate of change, and the smooth motion can reduce the energy consumption of the robot and reduce the mutation of dynamic load, wherein the path curvature peak value and the curvature rate of change are important factors, and the shortening of the path length is beneficial to reduce the motion time and improve the production efficiency of the robot, therefore, the square sum of the path curvature rate of change is used to construct an energy function, and the path length is weighted and summed to form a cost function, and the path curvature peak value and the safety distance are included in the constraint condition, and an initial path optimization model is constructed, and an initial collision-free fold line path with minimum total cost is searched;

[0012] Step S3, re-optimization of the initial path. Since the initial path uses a triangular grid pathfinding, the initial path obtained by planning has many redundant points, many turning times and large turning angles, which is not conducive to subsequent motion planning. Considering that the actual motion of the robot is not limited by the grid, therefore, the total cost of the initial fold line path is proposed as the initial value, and the initial path is further optimized through the re-optimization method of three steps of deleting acute angle vertex, path replacement and removing redundant path points;

[0013] Step S4, cubic NURBS fitting of the path and width realization. First, input the optimal fold line path point as the type value point, calculate the control point through the formula, select the appropriate weight factor, and the NURBS path can be determined. Since the path considers the safety distance, the NURBS path line is taken as the center line, and the width of 2 times the safety distance can obtain the NURBS path with width. This method improves the defect that the conventional algorithm idealizes the path as a zero-width line path and cannot safely avoid obstacles;

[0014] Step S5, five-segment S-shaped acceleration and deceleration motion control curve design of the trajectory tangential jerk satisfying the "Bang-Bang-Singular" strategy. First, according to the specific structure, characteristics and working conditions of the robot, the value limits of its speed, acceleration and jerk are determined. Then, based on the geometric shape, total length and robot motion strategy of the cubic NURBS path, the lengths and running times of each segment trajectory are determined, and the whole path is divided into five segments, which correspond to the acceleration segment, deceleration segment, constant speed segment, acceleration and deceleration segment and deceleration segment of the motion control respectively. Finally, the speed, acceleration and jerk control strategy of each segment trajectory is specifically designed.

[0015] The design contained in step S1 of the present application (a) corner method for judging whether a point is outside a polygon obstacle, and (b) standard for judging whether a whole or a part of a triangle in a triangular group network is inside an obstacle are as follows:

[0016] (a) Corner method for judging whether a point is outside a polygon obstacle;

[0017] Let the projection of the obstacle on the two-dimensional ground be a closed polygon, V i (i=1,2,…k) are the vertices of the polygon, and from any point P to V i (i=1,2,…k) are connected, and α i (i=1,2,…,k-1) are the connecting lines PV i to the connecting line PV i+1 is the angle of rotation (positive for counterclockwise, and negative for clockwise), and α k is the connecting line PV k is the angle of rotation back to the initial connecting line PV1. The specific requirement for judging that point P is outside the polygon obstacle according to the rotation angle method is:

[0018]

[0019] This formula indicates that starting from the initial connecting line PV1, the other connecting lines PV2, PV3,…, PV k are rotated in turn, and finally the initial connecting line PV1 is rotated back. If the sum of all rotation angles is zero, then point P is located outside the polygon obstacle; k

[0020] (b) The criterion for judging whether the whole or a part of a triangle in a triangular network is inside an obstacle;

[0021] Since it is not allowed to search for a path inside an obstacle, after triangulation, the triangulation triangles inside the obstacle need to be removed. If a part of a triangulation triangle is inside an obstacle, then the part of the line segment inside the obstacle is removed, and the boundary of the obstacle and its intersection with the triangle are included and form a new triangulation network. All free triangles in the new triangular network together constitute the free space;

[0022] Let there be n obstacles in the environment, barrier ξ is the ξth obstacle (ξ=1,2,…,n), and there are m triangles in the triangular network, is the ξth triangle is the ξth triangle Then the criterion for judging whether the whole or a part of a triangle is inside an obstacle barrier ξ is

[0023]

[0024] In the formula, is the area of the intersection of the triangle and the obstacle barrier ξ ;

[0025] ​The map environment constructed based on the above rules is shown in the following figure Figure 2 . Figure 2 The area where the unfilled triangle is located is free space, and the filled polygon is an obstacle. The circle and the pentagram are the markers of the start point and the goal point of the path, respectively. The midpoint of the free edge of the triangle network in the map is the optional path point for subsequent search, and the edge set A of the optional path is defined as the set of the connection lines of the midpoints of the two free edges of adjacent triangles, as well as the connection lines of the start and end points and the adjacent free edge midpoints. This is beneficial to the path away from the obstacle vertex and the boundary, and better realization of the path collision-free;

[0026] The initial path optimization model step in step S2 of the application is as follows:

[0027] (a) constructing an energy function;

[0028] The continuous curve path is discretized, and the energy function is constructed by the square sum of the curvature change rate, and the expression is

[0029]

[0030] In the expression, P1, P2, …, Pm are the discrete point sequence (m is the total number of discrete points, P1, P2, …, Pm are the start and end points of the path), κi is the curvature of the discrete point Pi, and Δli is the distance between adjacent discrete points, that is, Δli=|Pi-Pi+1|. m m i i i i i+1 i

[0031] P i i is the discrete curvature κi at P i , and the calculation formula is

[0032]

[0033] In the calculation formula, P i-1 , P i , and P i+1 represent three adjacent nodes in the path trajectory; represents the signed triangle ΔP i-1 P i P i+1 area, which is defined as positive when P i-1 , P i , and P i+1 run counterclockwise, and vice versa;

[0034] (b) constructing a cost function; ​​​​​​​​

[0035] The cost is the weighted sum of the energy function and the length indicator value to achieve the time-energy optimal path. The mathematical expression is as follows

[0036]

[0037] In the expression, λ κ and λ L respectively represent the weight coefficients of the discrete curvature indicator and the length (distance) indicator of each grid node; X represents the complete path to be evaluated; m represents the number of nodes from the starting point P1 to the target point P m .

[0038] (c) constructing an optimization model of the initial path;

[0039] The optimization model of the initial path is

[0040]

[0041]

[0042] In the optimization model, κ lim is the required trajectory curvature peak limit value of the mobile robot system. The optimization model requires 0≤λ κ ≤1, 0≤λ L ≤1, λ κ +λ L =1. Its objective function represents the minimum weighted sum of the energy and length of the path; the first constraint is the curvature of the path; the second is the trajectory curvature peak limit made considering that the mobile robot cannot continuously move at the turning point due to its own existence of nonholonomic constraint; the third constraint is that the distance D(P i ,barrier ξ ) between any node in the path and the obstacle is not less than the safety distance D safe , to ensure the collision-free and safety of the trajectory; the fourth and fifth constraints respectively represent that the path starts at P start and ends at P goal ; the sixth constraint is that the edges in the path trajectory all belong to the edge set A, A path is the selected edge set of the path.

[0043] (d) searching for an initial collision-free polyline path with the minimum total cost;

[0044] According to the optimization model of the initial path, based on the map environment constructed by using the Delaunay triangulation in step S1, the midpoint of the free edge of the triangle network in the map is used as the optional path point for subsequent search, the set of edges of the optional path is the set of lines connecting the midpoints of two free edges of adjacent triangles and the set of lines connecting the start point and the end point and the adjacent free edge midpoints, and the initial collision-free polyline path with the minimum total cost is searched from the start point to the target point;

[0045] The re-optimization of the initial polyline path in step S3 includes the following steps:

[0046] (a) deleting an acute angle vertex;

[0047] When there is an acute angle in the path, the acute angle vertex is sequentially tried to be deleted, and the line directly connecting the front and rear path points is used, and if the new path cost can be reduced and the curvature peak limit and the safety distance constraint are met, the new path is accepted; otherwise, the original path is retained;

[0048] (b) path replacement;

[0049] Starting from the starting point, the end point, the second last point, the third last point, …, the next next point of the path point are sequentially searched on the path directly connected to the path point, and it is judged whether the cost of the new path directly connected can be reduced and the curvature peak limit and the safety distance constraint are met, and if so, the direct line between the two points is used to replace the original path between the two points, otherwise, the original path is retained. Repeat iteration until the path replacement between the first point, the second point, the third point, …, the third last point, the second last point and the end point is completed;

[0050] (c) removing redundant path points;

[0051] When there are adjacent paths in a line, the intermediate redundant points are removed, and the two path points at the end are directly connected;

[0052] Through the repeated iteration of the above three steps, the re-optimized collision-free optimal polyline path is finally obtained. The algorithm is:

[0053] Algorithm 1: collision-free optimal polyline path planning algorithm based on Delaunay triangulation:

[0054] Input: start point, target point, coordinate values of edges and vertices of obstacles in the environment space, and some points are appropriately given or randomly generated outside the obstacles for Delaunay triangulation

[0055] Output: map environment, feasible path node set, initial and re-optimized path

[0056] 1: initialize parameters, initialize the OPEN_SET set and the CLOSE_SET set

[0057] 2: Establish feasible path node set based on Delaunay triangulation and obstacle polygon according to planning space prior knowledge

[0058] 3: Add start point to OPEN_SET

[0059] 4: while(OPEN_SET is not empty)

[0060] {if(end point is in OPEN_SET)

[0061] {return initial path of end point backtracking}

[0062] else

[0063] {

[0064] Add current processing node to CLOSE_SET

[0065] if(nodes around current processing node are not in CLOSE_SET

[0066] {Calculate cost

[0067] Add to OPEN_SET and sort by cost

[0068] Set parent node as current processing node;}

[0069] }

[0070] }

[0071] return no feasible path

[0072] 5: while(feasible path exists & there is a situation where the angle between path connecting lines in feasible path is acute)

[0073] {Try to remove vertex where acute angle is located one by one, and directly connect path points before and after the vertex

[0074] Calculate cost

[0075] if(cost of directly connected path is reduced & curvature is less than curvature peak limit value & distance to obstacle is not less than safety distance

[0076] {Remove vertex where acute angle is located from OPEN_SET

[0077] Optimize path nodes and update path

[0078] else

[0079] {Keep path nodes}

[0080] }

[0081] 6: while (there is a feasible path)

[0082] {from the start point of the path, search the direct connection between the end point, the second last point, the third last point, …, of its connection path in turn;

[0083] calculate its cost;

[0084] if (the cost of the direct connection path is reduced & the curvature is less than the curvature peak limit value & the distance from the obstacle is not less than the safety distance)

[0085] {remove all path points between the two points from the OPEN_SET;

[0086] replace the original path line between the two points with the direct connection between the two points;

[0087] path node optimization, path update}

[0088] else

[0089] {path node remains}

[0090] }

[0091] if (there is an acute angle between the adjacent path lines of the new path after replacement)

[0092] {return 5}

[0093] 7: while (there is a feasible path)

[0094] {if (the adjacent paths are collinear)

[0095] {remove the redundant end point at the middle of the collinear line from the OPEN_SET;

[0096] replace the original collinear path with the direct connection between the two end points at the end of the collinear part;

[0097] calculate its cost;

[0098] path node optimization, path update}

[0099] else

[0100] {path node remains}

[0101] }

[0102] 8: output the initial path of the mobile robot and the collision-free optimal polygonal path.

[0103] The trajectory tangential jerk of the step S5 meets the five-segment S-shaped acceleration-deceleration motion control curve design of "Bang-Bang-Singular" strategy. The step S5 comprises the following steps:

[0104] (a) according to the specific structure, characteristics and working conditions of the robot, the speed, acceleration and jerk value limit of the robot are determined;

[0105] (b) the length and running time of each segment trajectory are determined;

[0106] The five-segment S-shaped acceleration-deceleration segments are respectively acceleration acceleration segment, deceleration acceleration segment, constant speed segment, acceleration-deceleration segment and deceleration-deceleration segment. The velocity and acceleration along the trajectory tangential are continuous, and the jerk is "Bang-Bang-Singular" linear step form, and its distribution curve is as shown in Figure 3 .

[0107] Suppose the time spent by the five-segment motion is τ1, τ2, τ3, τ4, τ5, the motion end time is t1, t2, t3, t4, t5, and the length corresponding to each segment trajectory is s1, s2, s3, s4, s5; then the velocity, acceleration and jerk of the five-segment S-shaped acceleration-deceleration motion control curve and the relationship between time are as follows:

[0108]

[0109]

[0110]

[0111] The velocity planning needs to meet the initial and terminal state requirements, which are as follows:

[0112]

[0113]

[0114] Suppose the total time spent by the five-segment motion is t total , considering the symmetry of the curve, we can get:

[0115]

[0116] Suppose v lim , a lim , J lim are the limit values of velocity, acceleration and jerk. In order to obtain the smoothness of the motion, the motion of the mobile robot is required to meet the velocity, acceleration and jerk constraints:

[0117] v≤v lim

[0118] a≤alim

[0119] J≤J lim

[0120] According to the end acceleration of the first segment, we have:

[0121] Jt1≤a lim

[0122] Since t1=τ1=τ, we have:

[0123]

[0124] According to the end velocity of the second segment or the start velocity of the third segment, we have:

[0125]

[0126] Since t2=τ1+τ2=2*t1=2τ, we have:

[0127]

[0128] Considering the length constraint of the trajectory, let the total length of the trajectory be s. Integrating the velocity-time relationship and solving, we obtain the lengths s1, s2, s4, and s5 of the trajectory that the robot traverses during the first, second, fourth, and fifth velocity segments, respectively:

[0129]

[0130]

[0131] Therefore, the length s3 of the trajectory that the robot traverses during the uniform motion of the third segment is:

[0132] s3=s-2Jτ 3

[0133] According to s3≥0, we obtain s≥2Jτ 3 That is:

[0134]

[0135] Considering the acceleration, jump, and trajectory length constraints, the extreme value of time τ is:

[0136]

[0137] From the velocity-time relationship, we obtain the velocity v3 of the robot along the trajectory tangent during the uniform motion of the third segment:

[0138] v3=Jτ 2

[0139] Combining s3=s-2Jτ3 , the time t3 spent during the third uniform motion is:

[0140]

[0141] Combine The total time t spent on the five movements can be obtained total for:

[0142]

[0143] In this way, the length and running time of each segment trajectory can be determined.

[0144] (c) Divide the entire path into five segments. Based on the length and running time of each segment, the entire path is divided into five segments, corresponding to the acceleration segment, deceleration segment, constant speed segment, acceleration and deceleration segment, and deceleration segment of motion control.

[0145] (d) Specific design of the speed, acceleration, and jerk control strategies for each trajectory segment;

[0146] According to the extreme value of time τ is That is The smallest of the three is taken. After determining which of the three is the smallest, it is determined that in order to minimize the total motion time, the value of one of the parameters of trajectory length, speed, acceleration and jerk is restricted. The maximum value of the parameter is taken as the restricted value. Then, the specific speed, acceleration and jerk of each segment of the trajectory are calculated through the relationship between speed, acceleration and jerk and time of the five-segment S-shaped acceleration and deceleration motion control curve. The following further gives the relationship between τ and time. Among the three, the total time t is achieved when different minimum values ​​are obtained. total The minimal approach:

[0147] ① situation

[0148]

[0149] If s, v lim is a constant, then J=J lim Time, total time t total The minimum value is

[0150]

[0151] ② situation

[0152]

[0153] If s, a lim is a fixed value, then Time, total time ttotal Minimum, the value is

[0154]

[0155] ③ Case

[0156] In this case, s is a constant value, then J = J lim When the total duration t total Minimum, the value is

[0157]

[0158] Compared with the prior art, the application has the following beneficial technical effects:

[0159] 1) The map environment construction method based on Delaunay triangulation has strict mathematical definition, complete theoretical basis, excellent mathematical characteristics and geometric characteristics (such as empty outer circle characteristics, minimum angle maximum property, local modification, uniqueness, optimality and regularity). The midpoint of the free edge of the triangular network in the map is used as the optional path point for subsequent path search, and the edge set of the optional path is defined as the set of the connection lines of the midpoints of the two free edges of the adjacent triangles and the connection lines of the start and end points and the adjacent free edge midpoints, so that the path can be away from the obstacle vertex and the boundary, and the path collision avoidance is better achieved.

[0160] 2) For the time-energy-smoothing problem of the mobile robot path, a cost function covering the path energy and length is constructed, and a time-energy-optimal smoothing path search method of cubic NURBS collision avoidance is proposed, so that the path well meets the requirements of the mobile robot on the curvature peak value and the curvature change rate limit, effectively shortens the path length, reduces the number of turns and the turning angle, and realizes the optimization of the time-energy index.

[0161] 3) A five-segment S-shaped acceleration-deceleration motion control method is proposed under the Bang-Bang-Singular strategy of the trajectory tangential jerk, which realizes the shortest motion time and guarantees the kinematic performance of the mobile robot.

[0162] 4) The safety distance is considered, the trajectory has a width, and the defect that the conventional algorithm idealizes the path as a zero-width line path and cannot safely avoid obstacles is improved.

[0163] The application belongs to the path, trajectory and motion comprehensive planning technology, does not need to simplify the robot into a point, does not limit the robot working environment and whether the obstacle is regular, can process the search of any two-dimensional space path containing obstacles, guarantees the trajectory curve smoothing and the dynamics and flexibility performance of the motion system, and realizes the unified planning of geometry and motion. BRIEF DESCRIPTION OF DRAWINGS

[0164] Figure 1 Flow chart of the technical solution of the present application.

[0165] Figure 2 Example of robot environment modeling based on Delaunay triangulation. The area of the unfilled triangle is free space, and the filled polygon is an obstacle. The circle and pentagram are the markers of the start point and the goal point, respectively.

[0166] Figure 3 Tangential velocity, acceleration and jerk distribution curves of a five-segment S-shaped acceleration-deceleration motion trajectory.

[0167] Figure 4 Example of the initial path planning and node optimization process. Figure (a) is the initial path, figure (b) is the path after removing the acute-angle vertex, figure (c) is the path after the first replacement, and figure (d) is the path after removing the redundant points on the path. The remove in figure (d) is the marker of removing the redundant points on the path.

[0168] Figure 5 Comparison of the two optimal polyline paths of the present application with different weights and the schemes in documents [1] and [2].

[0169] Figure 6 Comparison of the optimal NURBS paths of the present application and the schemes in documents [1] and [2].

[0170] Figure 7 Comparison of the path refitted by the present application and the schemes in documents [1] and [2].

[0171] Figure 8 Implementation of the path width of the present application.

[0172] Figure 9 Segmentation of the trajectory according to the difference in motion law.

[0173] Figure 10 Jerk of the trajectory.

[0174] Figure 11 Position vector and direction in the motion process.

[0175] Figure 12 Displacement of x, y and the direction of the trajectory.

[0176] Figure 13 Velocity of x, y and the direction of the trajectory.

[0177] Figure 14 Acceleration-time variation diagram.

[0178] Figure 15The trajectory velocity and acceleration planning of the reference [1]. DETAILED DESCRIPTION

[0179] The present invention will be further described below with reference to the accompanying drawings by way of embodiments.

[0180] (1) Environment and parameter settings of this embodiment

[0181] The algorithm is run in Windows 10 64-bit, Matlab 2018a, i7-7700HQ processor, and 16GB memory. The kinematic constraints in this case are set to J lim =1500m / s 3 、a lim =200m / s 2 、v lim =20m / s,κ lim =2 / m, safety distance D safe =0.1m.

[0182] To verify the effectiveness of the proposed time-energy optimal smooth trajectory planning method based on cubic NURBS, we considered that Jin Zhirong and Shen Liyong's paper "Time-Optimal Trajectory Planning Based on Quadratic B-Splines" (hereinafter referred to as Document 1) and Giannelli's paper "Path Planning with Obstacle Avoidance by G1 PH QuinticSplines" (hereinafter referred to as Document 2) have conducted in-depth research in this area and achieved a series of results. The experimental environment, obstacle, starting point, and target point locations of the present invention are derived from the data in Documents 1 and 2, and the results of the present invention are compared with those of the two documents.

[0183] (2) Map environment construction based on Delaunay triangulation.

[0184] The robot's task space is determined to be a 7m×11m rectangle with 6 polygonal obstacles. The coordinates of the starting point are (2.4, 3.8) and the coordinates of the target point are (10.6, 3.75). The vertex coordinates of obstacle 1 are (1.2, 3.5), (2, 4), (2.5, 5.5), and (1, 6); the vertex coordinates of obstacle 2 are (3.2, 2.8), (4.1, 4.5), and (2.8, 4.5); the vertex coordinates of obstacle 3 are (5.1, 4.1), (6.8, 3.8), (7.3, 6.1), and (4.7, 5.6); the vertex coordinates of obstacle 4 are (3.1, 1.1), (4.7, 1.6), (4.2, 2.8), and (2.4, 2); the vertex coordinates of obstacle 5 are (8, 1.8), (7.3, 1.8), and (5.9, 3.2); and the vertex coordinates of obstacle 6 are (7.9, 2.5), (10, 2.1), (9.8, 3.9), (8.8, 4.1), and (7.8, 3.6). The six obstacles are all convex polygons formed by connecting vertices.

[0185] Design a Delaunay triangulation algorithm. First, triangulate the task space. Then define obstacles. Based on the criteria of determining whether a point is outside the polygonal obstacle based on the corner rotation method, and determining whether the entire triangle or a part of the triangle in the triangulation network is inside the obstacle, remove the triangulation triangles inside the obstacle and include the obstacle boundary to form a new triangulation network. The specific triangulation network, obstacles and task environment are shown in the attached figure. Figure 2 .

[0186] Next, the midpoints of the free edges of the triangle network in the map are defined as optional path points for subsequent path searches; the edge set of the optional path is defined as the set of lines connecting the midpoints of the two free edges of adjacent triangles, and the set of lines connecting the starting and ending points with the midpoints of the adjacent free edges, providing data for subsequent path searches.

[0187] (3) Construct energy function, cost function and perform initial broken line path planning

[0188] Discrete curvature κ i Defined as:

[0189]

[0190] In formula (1), P i-1 、P i 、P i+1 represents three adjacent nodes in the discrete path trajectory; κ i is a discrete point P i The curvature of a point, Represents the signed triangle ΔP i-1 P i P i+1Area, when P i-1 , P i , P i+1 is defined as positive when running counterclockwise, and vice versa.

[0191] Let P1, P2, …, P m be a sequence of discrete points (m is the total number of discrete points, P1, P m are the start and end points of the path, respectively), and the energy function of the path is constructed as:

[0192]

[0193] The cost is the weighted sum of the energy function and the length index value, and its expression is:

[0194]

[0195] In equation (3), λ κ and λ L represent the weight coefficients of the discrete curvature index and the length (distance) index of each grid node, respectively; X represents the complete path to be evaluated; and m represents the number of nodes from the start point P1 to the target point P m .

[0196] The optimization model of the initial path is constructed as:

[0197]

[0198] In equation (4), κ lim is the required trajectory curvature peak limit value of the mobile robot system. The optimization model requires 0≤λ κ ≤1, 0≤λ L ≤1, and λ κ +λ L =1. Its objective function represents the minimum weighted sum of the energy and length of the path; the first constraint is the curvature of the path; the second constraint is the trajectory curvature peak limit considering that the mobile robot cannot continuously move at the turning point due to its own non-holonomic constraint; the third constraint is that the distance D(P i ,barrier ξ ) between any node in the path and the obstacle is not less than the safety distance D safe , to ensure the collision-free and safety of the trajectory; the fourth and fifth constraints represent that the path starts at P start and ends at P goal ; and the sixth constraint is that the edges in the path trajectory all belong to the edge set A, A path is the selected edge set of the path.

[0199] In the optimization model of the initial path of the embodiment, the trajectory curvature peak limit value κ lim=2 / m, safety distance D safe =0.1m, starting point P start The coordinates are (2.4, 3.8), the end point P goal The coordinates are (10.6,3.75).

[0200] According to the optimization model (Formula (4)), the initial collision-free polyline path with the minimum total cost is searched from the starting point to the target point. L =1,λ κ = 0, the initial broken line path obtained by searching is as shown in the attached figure. Figure 4 As shown in (a).

[0201] (4) Optimization of the initial broken line path

[0202] For λ L =1,λ κ = 0, the initial polyline path obtained by searching is optimized by three steps: deleting sharp corner vertices, replacing paths, and removing redundant path points. The path obtained after deleting sharp corner vertices is shown in the attached figure. Figure 4 (b) The path after path replacement is shown in the attached Figure 4 (c) The path after removing redundant path points is shown in the attached figure. Figure 4 (d) shown.

[0203] Attachment Figure 4 The initial path in (a) goes through 24 points (including the starting point and the target point), with a total length of 11.7466 and an energy of 16.0289; Figure 4 (b) Delete the vertices of the acute angle (counting from the starting point, the 3rd, 7th, 9th, and 22nd points on the path). The resulting path goes through 20 points, with a total length of 10.0569 and an energy of 8.4155. Figure 4 (c) Based on the path with the acute angle vertex deleted, the 6th, 7th, 9th, 10th, 12th, 13th, 14th, 15th, 16th, 18th, and 19th points of the new path are removed. The path after the initial replacement has 9 points, a total length of 9.4254, and an energy of 1.6264. Figure 4 (d) After removing the redundant point (point 4) on the path, the path passes through 8 points, with a total length of 9.4254 and an energy of 1.0963.

[0204] By comparison, the optimized broken line path effectively eliminates sharp-angle vertices, redundant points and broken lines, shortens the planned path length, reduces curve energy and turning angles, and significantly improves the time-energy performance of the path.

[0205] Further changes to λ L and λ κ The value of can be used to plan the path under different weights. Figure 5 Given λ L= 1 and λ κ = 1, see the solid line (referred to as path 1) and the dotted line (referred to as path 2) in the attached Figure 5

[0206] Now, the two paths of the present application are compared with the optimal polygonal paths in documents [1], [2] (the attached Figure 5 ). When the time optimality (shortest length) is absolutely valued, λ L = 1 is taken, the optimal polygonal path 1 is searched by the present application, and the total length is 9.4254 m. Compared with the time optimal path scheme in document [1] (the attached Figure 5 , the path passes through 5 points, and the total length is 9.7006 m), the path of the present application is shortened by 2.837%; but compared with the polygonal path of the VGSP algorithm in document [2] (the dotted line in the attached Figure 5 , the path passes through 7 points, and the total length is 8.7656 m), it is increased. The reason is that the path in document [1] (also in document [2]) is simplified as a line path with a width of 0, which is closely attached to the edge or vertex of the obstacle, and obviously cannot guarantee the safety distance of the robot during movement. By carefully comparing the path 1 of the present application with the path of document [2], the two paths are basically consistent, and the main reason for the longer path 1 of the present application is that the width of the path is considered, the safety distance of not less than 0.1 m is realized, and the safety of the robot passing is effectively guaranteed. From the safety point of view, the optimal paths in documents [1], [2] are not feasible.

[0207] When the curve energy optimality is absolutely valued, λ κ = 1 is taken, the optimal polygonal path 2 is searched by the present application, passes through 4 points, and the total length is 10.5548 m, and the energy is 0.0166, and the minimum distance between the points on the path and the obstacle is 0.1973 m. Compared with 0.0425 in document [1] and 0.3590 in document [2], the path energy is reduced by 60.94% and 95.38% respectively. It can be seen that the method of the present application effectively realizes the lowest energy while guaranteeing the safety of the robot passing.

[0208] (5) NURBS fitting of the polygonal path

[0209] By using the method of the present application, the cubic NURBS fitting is performed on the polygonal paths 1, 2, and the results are shown in the attached Figure 6 . It can be seen that the path of the present application has the characteristics of good smoothness, few turning points, few turning times, and small turning angles.

[0210] The attached Figure 6 ​The optimal NURBS path 2 (dotted line, corresponding to the broken line path 2) of the present invention is 11.3258m long. This is mainly because there are only 4 shape value points on the path, which is too few. The present invention attempts to add the 0.25 and 0.85 scale quantiles on the second broken line of the broken line path as shape value points and refit to obtain a new NURBS path, see the attached figure. Figure 7 The dotted line in the figure reduces the path length to 10.8036 m. As can be seen, while shortening the path, it still retains good curvature characteristics and ensures sufficient safety distance.

[0211] (6) NURBS path width implementation

[0212] Since the NURBS path of the present invention takes the safety distance into consideration, a NURBS path with a width can be obtained by taking the NURBS path line as the center line and 2 times the safety distance as the width. Figure 8 This method can improve the defect of conventional algorithms that the path is idealized as a zero-width line path and cannot safely avoid obstacles.

[0213] (7) Trajectory motion planning

[0214] Taking the cubic NURBS path 1 as an example, according to the five-segment S-shaped acceleration and deceleration motion control method designed by the present invention, the robot starts from the starting point (start) in a static state, goes through five speed segments in sequence, and finally reaches the target point goal. Figure 9 The diamonds on the medium thick solid line are the segmentation points of the trajectory according to the law of motion. Each segment corresponds to the acceleration segment, deceleration segment, constant speed segment, acceleration and deceleration segment, and deceleration segment, with corresponding lengths of 0.3849m, 1.9245m, 4.9768m, 1.9245m, and 0.3849m respectively; the time taken is 0.1155s, 0.1155s, t3 = 0.2488s, 0.1155s, and 0.1155s respectively; the total time taken is 0.7107s.

[0215] Attachment Figure 10 The figure below shows the time-varying jerk in the trajectory direction. It shows that the jerk along the entire trajectory satisfies the "Bang-Bang-Singular" control strategy. Jerk remains at its peak (or valley) value (Jerk = 1500 m / s³) throughout the acceleration, deceleration, acceleration, and deceleration segments, while remaining at zero during the uniform velocity segment. The prolonged saturation of the jerk at its peak (or valley) value facilitates rapid control response and energy optimization.

[0216] Attachment Figure 11 The position vector and direction of the robot during the motion of the present invention are given, and it can be seen that it is smooth. Figure 12For displacement maps of x, y and trajectory direction at different times, it is obvious that the displacement curves of each direction are smooth.

[0217] attached Figure 13 and attached Figure 14 The x, y and trajectory direction velocities and accelerations under the planning of the present application are given respectively. It can be seen that the velocities and accelerations of each direction at the starting point and the target point are all zero, which meets the boundary conditions of the trajectory ends. The velocities of each direction on the whole trajectory are smooth, and the accelerations are continuous. Compared with the velocity of the trajectory in document [1] which is not smooth and the acceleration which is not continuous (attached Figure 15 ), the present application has obvious advantages in realizing smooth motion.

[0218] The path space shape and trajectory kinematics information planned by the present application and its comparison with the schemes in documents [1] and [2] verify the smoothness, safety and efficiency of the trajectory of the present application.

[0219] The path and trajectory planning are carried out by randomly changing the shape and position of the robot space and obstacles for many times, and good time-energy performance is achieved. It shows that the present application method can process the search of any two-dimensional space path containing obstacles, ensure the smoothness of the trajectory and the dynamics and flexibility performance of the system during motion, and realize the unified planning of geometry and motion.

[0220] In summary, the present application proposes a robot time-energy optimal smooth trajectory planning method based on cubic NURBS for the time-energy-smoothness problem of the path of the mobile robot with nonholonomic constraints and unable to continuously move at the turning point, the problem of zero-width line path unable to safely avoid obstacles, and the problem of five-segment S-shaped acceleration and deceleration control of trajectory motion. The present application constructs an energy function with the square sum of the path curvature change rate, constructs a cost function with the weighted sum of the energy function and the length index value, introduces the path curvature peak value and the safety distance constraint to build an optimization model, and carries out time-energy optimal path search in the Delaunay triangulation environment. Through re-optimization operations such as deleting acute-angle vertices, path replacement and removing redundant points, and NURBS fitting processing, the obtained path not only well meets the requirements of the mobile robot for the trajectory curvature peak value and the curvature change rate limit, the time-energy optimal target, but also considers the safety distance, realizes the path with width, and improves the defect that the conventional algorithm idealizes the path as a zero-width line path which cannot safely avoid obstacles. In order to further improve the stability and efficiency of the robot work, a five-segment S-shaped acceleration and deceleration motion control method is proposed, which satisfies the "Bang-Bang-Singular" strategy of the trajectory tangential jerk. Simulation and experimental verification show that the present application method can process the search of any two-dimensional space path containing obstacles, ensure the smoothness of the trajectory curve and the dynamics and flexibility performance of the system during motion, and realize the unified planning of geometry and motion. The specific conclusions are as follows:

[0221] 1) The map environment construction method based on Delaunay triangulation has strict mathematical definition, complete theoretical basis, excellent mathematical characteristics and geometric characteristics (such as empty outer circle characteristics, minimum angle maximum property, local modification, uniqueness, optimality and regularity), no special requirements for robot task space and obstacle shape and accurate description. The midpoint of the free edge of the triangular network in the map is selected as the optional path point for subsequent path search, and the edge set of the optional path is defined as the connection of the midpoints of the two free edges of the adjacent triangles, and the connection of the start and end points and the adjacent free edge midpoints, which can make the path away from the obstacle vertex and the boundary, and better realize the collision-free path.

[0222] 2) For the time-energy-smoothing problem of mobile robot path, a cost function covering path energy and length is constructed, and a cubic NURBS collision-free time-energy optimal smoothing path search method is proposed, which well meets the requirements of mobile robot for trajectory curvature peak value and curvature change rate limit, effectively shortens the path length, reduces the turning number and turning angle, and realizes the optimization of time-energy index.

[0223] 3) A five-segment S-shaped acceleration and deceleration motion control method is proposed under the "Bang-Bang-Singular" strategy of trajectory tangent jerk, which realizes the shortest motion time and guarantees the kinematic performance of the mobile robot.

[0224] 4) Considering the safety distance, the path has a width, which improves the defect that the conventional algorithm simplifies the robot as a point and idealizes the path as a zero-width line path, which cannot safely avoid obstacles.

[0225] The present application belongs to the field of robot path, trajectory and motion comprehensive planning technology, which does not need to simplify the robot as a point, does not limit the robot working environment and whether the obstacle is regular, can process the search of any two-dimensional space path containing obstacles, guarantees the trajectory curve smoothing and the dynamics and flexibility performance of the system during motion, and realizes the unified planning of geometry and motion.

Claims

1. A robot time-energy optimal smooth trajectory planning method, characterized by Follow these steps: Step S1: Map environment construction based on Delaunay triangulation: This includes determining the coordinate values ​​of the edges and vertices of obstacles in the task space, removing the triangles inside the obstacles based on Delaunay triangulation, and incorporating the boundaries of the obstacles to form a new triangulation network; including the design of: (a) The criterion for determining whether a point is outside a polygonal obstacle using the corner method; (b) Criteria for determining whether a triangle in a triangulated network is entirely or partially inside an obstacle; Define the midpoints of the free edges of the triangle network in the map as optional path points for subsequent path search; the edge set of the optional path is the set of lines connecting the midpoints of the two free edges of adjacent triangles, and the set of lines connecting the starting and ending points with the midpoints of the adjacent free edges; Step S2, initial polyline path planning: An energy function is constructed using the square sum of the path curvature change rate, and this function is combined with the weighted sum of the path length to form a cost function. The path curvature peak and safety distance are also included in the constraints to construct an initial path optimization model and search for the initial collision-free polyline path with the minimum total cost. Step S3, re-optimizing the initial path: taking the total cost of the initial polyline path as the initial value, the initial path is further optimized by deleting sharp-angle vertices, replacing the path, and removing redundant path points; Step S4, cubic NURBS fitting of the path and width realization: first input the optimal polyline path points as the type value points, inversely calculate the control points through the formula, select the appropriate weight factor, determine the NURBS path, then use this NURBS path line as the center line and 2 times the safety distance as the width to obtain the NURBS path with the width; Step S5. First, according to the specific structure, characteristics and working conditions of the robot, determine the speed, acceleration and jerk value limits; then, based on the geometric shape, total length and robot motion strategy of the cubic NURBS path, determine the length and running time of each segmented trajectory, and divide the entire path into five segments, corresponding to the acceleration segment, deceleration segment, uniform speed segment, acceleration and deceleration segment and deceleration segment of motion control; finally, specifically design the speed, acceleration and jerk control strategy for each segment of the trajectory.

2. A robot time-energy optimal smooth trajectory planning method according to claim 1, characterized in that the criterion for determining whether a point is outside a polygonal obstacle by the angle method (a) in step S1 is: Assume that the projections of obstacles on the two-dimensional ground are all closed polygons, V i , i=1,2,…k are the vertices of the polygon, from any point P to V i , i=1,2,…k are connected respectively, α i , i=1,2,…,k-1 is the line PV i Direction PV i+1 The angle of rotation is positive if it is counterclockwise and negative if it is counterclockwise. k To connect PV k Return to the angle of the initial line PV1; the specific requirements for determining whether point P is outside the polygonal obstacle according to the angle rotation method are: This formula indicates that starting from the initial line PV1, it moves to other lines PV2, PV3, ..., PV k Rotation, finally by PV k Return to the initial line PV1. If the sum of all the angles cancels out to 0, then point P is outside the polygonal obstacle. The criteria for determining whether a triangle in a triangular network is entirely or partially inside an obstacle in (b) are: Since it is not allowed to find a path inside an obstacle, after triangulation, the triangles inside the obstacle need to be removed. If a part of the triangle is inside the obstacle, the line segment inside the obstacle is removed, and the boundary of the obstacle and its intersection with the triangle are included to form a new triangulation network. All free triangles in the new triangulation network together constitute the free space. Assume there are n obstacles in the environment, barrier ξ is the ξth obstacle, ξ=1,2,…,n, and there are m triangles in the triangular network. For the triangles, but The whole or part of the barrier ξ The internal judgment criteria are Where, is a triangle and barriers ξ The area of ​​the intersection.

3. A robot time-energy optimal smooth trajectory planning method according to claim 1, The characteristic is that the initial path optimization model steps in step S2 are as follows: (a) Constructing the energy function Discretize a continuous curve path and construct an energy function using the sum of the squares of the curvature change rate. The expression is: In the expression, P1, P2, ..., P m is a discrete point sequence, m is the total number of discrete points, P1, P m are the starting and ending points of the path, κ i is a discrete point P i The curvature of a point, Δl i is the distance between adjacent discrete points, Δl i =|P i+1 -P i |; P i The discrete curvature κ at i The calculation formula is: In the calculation formula, P i-1 、P i 、P i+1 Represents three adjacent nodes in the path trajectory; Represents the signed triangle ΔP i-1 P i P i+1 Area, when P i-1 、P i 、P i+1 When running counterclockwise, it is defined as positive, and vice versa as negative; (b) Constructing the cost function The cost is the weighted sum of the energy function and the length index value to achieve the time-energy optimization of the path; the expression is as follows: In the expression, λ κ and λ L Represents the weight coefficients of the discrete curvature index and length index of each grid node; X represents the complete path to be evaluated; m represents the distance from the starting point P1 to the target point P m The number of nodes; (c) Constructing an optimization model for the initial path The optimization model of the initial path is: In the optimization model, κ lim is the peak curvature limit of the trajectory required by the mobile robot system; the optimization model requires 0≤λ κ ≤1, 0≤λ L ≤1,λ κ +λ L =1; Its objective function represents the minimum weighted sum of the energy and length of the path; the first constraint is the curvature of the path; the second is the peak curvature limit of the trajectory, which is made considering that the mobile robot cannot move continuously at the turning point due to its own non-holonomic constraints; the third constraint is the distance D (P i ,barrier ξ ) are not less than the safety distance D safe , to ensure the collision-free and safety of the trajectory; the fourth and fifth constraints represent the path with P start As the starting point, with P goal is the end point; the sixth constraint is that the edges in the path trajectory belong to the edge set A, A path The set of edges selected for the path; (d) Search for the initial collision-free polyline path with the minimum total cost According to the optimization model of the initial path, based on the map environment constructed using Delaunay triangulation in step S1, the midpoints of the free edges of the triangle network in the map are used as optional path points for subsequent searches, and the set of lines connecting the midpoints of the two free edges of adjacent triangles, as well as the set of lines connecting the starting and ending points with the midpoints of the adjacent free edges are used as the edge set A of the optional path. The initial collision-free broken line path with the minimum total cost is searched from the starting point to the target point.

4. A robot time-energy optimal smooth trajectory planning method according to claim 1, The method is characterized in that the initial broken line path re-optimization in step S3 comprises the following steps: (a) Delete the sharp corners; When there are sharp angles in the path, try to delete the sharp angle vertices one by one and directly connect the previous and next path points. If the new path cost can be reduced and meets the curvature peak limit and safety distance constraints, then accept it; otherwise, keep the original path; (b) Path replacement; Starting from the starting point, the algorithm searches for the endpoint, the second-to-last point, the third-to-last point, and so on, and the next point of the next point on the path directly connected to the path point. It determines whether the cost of the new path directly connected to the path point can be reduced and meets the curvature peak limit and the safety distance constraint. If so, the original path between the two points is replaced by the direct line between the two points. Otherwise, the original path is retained. Iterate repeatedly until the path between the first point, the second point, the third point, the second-to-last point, and the endpoint is completely replaced. (c) Eliminate redundant path points; When there are adjacent paths that are collinear, the middle redundant points are removed and the two path points at the end are directly connected; After repeated iterations of the above three steps, the optimized collision-free optimal broken line path is finally obtained.

5. A robot time-energy optimal smooth trajectory planning method according to claim 1, characterized in that Step S5 includes the following steps: (a) Determine the limits of the robot's speed, acceleration, and jerk based on its specific structure, characteristics, and operating conditions; (b) Determine the length and running time of each segment trajectory; The five S-shaped acceleration and deceleration sections are acceleration section, deceleration section, constant speed section, acceleration and deceleration section, and deceleration and deceleration section. The speed and acceleration along the trajectory tangent are continuous, and the jump is a Bang-Bang-Singular linear step form. Assume that the duration of the five-segment motion is τ1, τ2, τ3, τ4, and τ5, and the time when the motion ends is t1, t2, t3, t4, and t5 respectively. The length of each trajectory is s1, s2, s3, s4, and s5 respectively. The relationship between the speed, acceleration, and jerk and time of the five-segment S-shaped acceleration and deceleration motion control curve is as follows: Speed ​​planning must meet the initial and final state requirements, specifically: Let the total time spent on the five movements be t total , considering the symmetry of the curve, we can get: Let v lim 、a lim 、J lim are the limiting values ​​of speed, acceleration and jerk respectively; in order to obtain smooth motion, the motion of the mobile robot is required to satisfy the speed, acceleration and jerk constraints: v≤v lim a≤a lim J≤J lim According to the acceleration at the end of the first segment, we have: Jt1≤a lim Since t1=τ1=τ, we have: According to the speed at the end of the second segment or the starting point of the third segment, we have: Since t2=τ1+τ2=2*t1=2τ, we have: Consider the trajectory length constraint again; let the total length of the trajectory be s, integrate and solve the trajectory velocity-time equation, and find that during the first, second, fourth, and fifth speed segments, the segmented trajectory lengths s1, s2, s4, and s5 of the robot are: Therefore, during the third period of uniform motion, the length of the trajectory s3 traveled by the robot is: s3=s-2Jτ 3 According to s3≥0, we get s≥2Jτ 3 , that is: Taking into account the constraints of acceleration, jerk, and trajectory length, the extreme value of time τ is: According to the velocity-time relationship, the velocity v3 of the robot running tangentially along the trajectory during the third uniform motion period can be obtained as: v3=Jτ 2 Combined s3 = s-2Jτ 3 , the time t3 spent during the third uniform motion is: Combine The total time t spent on the five movements can be obtained total for: Thus, the length and running time of each segment trajectory can be determined; (c) Divide the entire path into five segments: Based on the length and running time of each segment trajectory, the entire path is divided into five segments, corresponding to the acceleration segment, deceleration segment, constant speed segment, acceleration and deceleration segment, and deceleration segment of motion control; (d) Specific design of the speed, acceleration, and jerk control strategies for each trajectory segment; According to the extreme value of time τ is That is Take the minimum of the three. After determining which of the three is the minimum, determine which parameter of trajectory length, speed, acceleration and jerk is restricted in order to minimize the total motion time. Take the maximum value of the parameter as the limit value. Then, calculate the specific speed, acceleration and jerk of each segment of the trajectory through the relationship between speed, acceleration and jerk and time of the five-segment S-shaped acceleration and deceleration motion control curve; τ is respectively Among the three, the total time t is achieved when different minimum values ​​are obtained. total The minimal approach: ① situation If s, v lim is a constant, then J=J lim Time, total time t total The minimum value is: ② situation If s, a lim is a fixed value, then Time, total time t total The minimum value is: ③ situation In this case, s is a constant, so J = J lim Time, total time t total The minimum value is:

Images