A path optimization method and system fusing TEB and RVO algorithms

Abstract

The application relates to a path optimization method and system fusing a TEB and an RVO algorithm, and belongs to the technical field of path planning of unmanned intelligent vehicles. The method comprises the following steps: obtaining a global path of an unmanned intelligent vehicle by using an A-star algorithm; planning a local trajectory by using a TEB algorithm; adding four target constraint functions in the TEB algorithm when the local trajectory is planned, so as to reduce turning points in the path and make the path more stable; under the premise of guaranteeing the trajectory as much as possible, reducing parameters required by the TEB algorithm, and reducing the calculation resources occupied in path planning; and performing collision judgment on the local trajectory obtained by the TEB algorithm by using an RVO algorithm, and performing secondary constraint on the obstacle avoidance speed of the unmanned intelligent vehicle near a dynamic obstacle, so that the unmanned vehicle can reasonably and efficiently solve the collision conflict problem, the trajectory of the unmanned intelligent vehicle tends to be smooth, the speed output is more reasonable, and unknown obstacles can be avoided in real time.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning for unmanned intelligent vehicles, and in particular relates to a path optimization method and system integrating TEB and RVO algorithms. Background Art

[0002] In recent years, autonomous vehicles have been increasingly used in numerous fields, including industry and daily life, becoming a key development in future intelligent manufacturing and smart cities. Path planning is a fundamental aspect of autonomous vehicle control systems and a prerequisite for them to successfully complete various tasks. Path planning is primarily categorized into global and local planning, depending on the level of familiarity with the surrounding environment.

[0003] Global path planning requires the input of a static map in advance and the prior knowledge of static obstacles in order to plan an optimal path. However, global algorithms perform poorly when the map is large or when unknown or dynamic obstacles appear in the map. Local path planning is used in situations where the surrounding environment is partially known or completely unknown, focusing on measuring the surrounding environment. However, due to the limited detection range of sensors, it is prone to falling into minimum points and failing to plan an optimal path. Therefore, in practical applications, a single algorithm has limitations when solving path planning problems and cannot handle the emergencies faced by unmanned intelligent vehicles during movement. Therefore, two algorithms are usually used in combination to obtain better planning results.

[0004] For the global planning algorithm, the present invention adopts the A* (short for A-star) global algorithm, which can not only effectively solve the problem of high computational complexity, but also plan an optimal route. For the local planning algorithm, the TEB (short for Time-Elastic-Band) algorithm is usually used. This algorithm not only adds the calculation of time information but also comprehensively considers the kinematic limitations of the unmanned intelligent vehicle. Therefore, it has a certain adaptability to different types of unmanned intelligent vehicles. However, in actual applications, due to the constraint function of the optimization objective, when the unmanned intelligent vehicle faces a collision conflict or special circumstances, the algorithm program may break through the hard constraints and convert into soft constraints, resulting in the unmanned intelligent vehicle having a poor trajectory and frequent speed jumps during dynamic obstacle avoidance, which lacks robustness. Summary of the Invention

[0005] In response to the above problems, the present invention provides a path optimization method that integrates the TEB and RVO algorithms. The path optimization is performed after global planning by the A-star algorithm. The TEB local planning algorithm and the RVO (abbreviation for Return-Value-Optimization) algorithm are integrated into the path optimization process. This method can solve the needs of frequent speed jumps of unmanned intelligent vehicles and real-time obstacle avoidance in complex environments.

[0006] The first object of the present invention is to provide a path optimization method that integrates TEB and RVO algorithms, comprising:

[0007] Construct a mathematical model for unmanned intelligent vehicles;

[0008] Establish a static map of the environment through the grid method, and obtain the current position and global target position of the unmanned intelligent vehicle on the static map of the environment;

[0009] Obtain the global path of the unmanned intelligent vehicle through the A-star algorithm;

[0010] The local trajectory is planned for the global path through the TEB algorithm;

[0011] Performing feasibility judgment on the local trajectory, wherein the feasibility judgment is to judge whether the local trajectory is feasible;

[0012] If the feasibility judgment result is no, return to and loop the steps of "obtaining the global path using the A-star algorithm", "planning the local trajectory using the TEB algorithm", and "judging the feasibility of the local trajectory" until the feasibility judgment result is yes;

[0013] If the feasibility determination result is yes, a collision determination is performed on the local trajectory, wherein the collision determination is to determine whether the local trajectory is within a collision zone of the new obstacle, where the collision zone of the new obstacle is calculated using the RVO algorithm.

[0014] If the collision judgment result is yes, return to and loop through the steps of “TEB algorithm planning local trajectory”, “feasibility judgment of local trajectory”, and “collision judgment of local trajectory” until the collision judgment result is no;

[0015] If the collision judgment result is negative, determine whether the unmanned intelligent vehicle has reached the target position;

[0016] Based on the result of whether the target position has been reached, the process returns to and loops through the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment", "local trajectory collision judgment", and "judging whether the unmanned intelligent vehicle has reached the target position" until the target position is reached.

[0017] Based on the judgment result of reaching the target location, the path optimization is completed.

[0018] In a specific embodiment of the present invention, the posture includes the position information and speed direction information of the unmanned intelligent vehicle.

[0019] In a specific embodiment of the present invention, when the unmanned intelligent vehicle is a four-wheel pendulum suspension unmanned intelligent vehicle, during the process of establishing the static map of the environment, the default parameter values ​​of the cost map resolution and the expansion coefficient of the unmanned intelligent vehicle are modified.

[0020] In a specific embodiment of the present invention, planning a local trajectory for a global path using the TEB algorithm includes:

[0021] The global path is converted into initial trajectory points through the TEB algorithm;

[0022] Based on the initial trajectory point, the starting position and posture are completed to reach the target position and posture under four target constraint functions, including path following and obstacle constraint function, speed and acceleration constraint function, non-holonomic kinematic constraint function and fastest path constraint function.

[0023] In a specific embodiment of the present invention, the expressions of the path following and obstacle constraint functions are as follows:

[0024]

[0025] Among them, f path and f ob is the penalty function, x Γ is the boundary, ε is the offset factor, S is the scaling factor, n is the order, d min,j is the independent variable, indicating the distance between the path and the obstacle, r pmax is the maximum distance the trajectory deviates from the path, r omin Indicates the minimum distance between the trajectory and the obstacle;

[0026] The expressions of the velocity and acceleration constraint functions are as follows:

[0027]

[0028] Among them, v i is the linear velocity of the unmanned intelligent vehicle at time i, ΔT i is the time interval between adjacent pose points, (x i ,y i ) is the coordinate of the unmanned intelligent vehicle at time i, ω i is the angular velocity of the unmanned intelligent vehicle at time i, β i is the orientation angle of the unmanned intelligent vehicle at time i, a i is the linear acceleration of the unmanned intelligent vehicle at time i.

[0029] In a specific embodiment of the present invention, the expression of the non-holonomic kinematic constraint function is as follows:

[0030]

[0031] Among them, f k (X i ,X i+1 ) is the objective function, di,i+1 is the direction vector, (x i ,y i ) is the coordinate of the unmanned intelligent vehicle at time i, β i is the orientation angle of the unmanned intelligent vehicle at time i.

[0032] The expression of the fastest path constraint function is as follows:

[0033]

[0034] Where, ΔT i is the time interval between adjacent pose points.

[0035] In a specific embodiment of the present invention, the collision determination of the local trajectory based on the feasibility determination result being yes includes:

[0036] If the feasibility judgment result is yes, check whether new obstacles appear in the local map;

[0037] Based on the judgment result of whether a new obstacle appears, choose whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle;

[0038] Determine whether the local trajectory is within the collision zone of the new obstacle.

[0039] In a specific embodiment of the present invention, the step of selecting whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle based on the result of determining whether a new obstacle has appeared includes:

[0040] Based on the judgment result of the new obstacle, the collision area of ​​the new obstacle is calculated using the RVO algorithm;

[0041] Based on the judgment result that no new obstacles appear, the steps of "the unmanned intelligent vehicle moves toward the target posture" and "determine whether the unmanned intelligent vehicle has reached the target posture" are performed in sequence.

[0042] A second object of the present invention is to provide a path optimization system integrating TEB and RVO algorithms, comprising:

[0043] Simulation module: used to build a mathematical model of the unmanned intelligent vehicle; and used to create a static map of the environment through the grid method, and obtain the current position and global target position of the unmanned intelligent vehicle on the static map of the environment;

[0044] A-star algorithm module: used to obtain the global path of the unmanned intelligent vehicle through the A-star algorithm;

[0045] Planning local module: used to plan local trajectories for the global path through the TEB algorithm;

[0046] Feasibility judgment module: used to judge the feasibility of the local trajectory, wherein the feasibility judgment is to judge whether the local trajectory is feasible; and used to return to and loop the "A-star algorithm to obtain the global path" and "TEB algorithm to plan the local trajectory" and "local trajectory feasibility judgment" steps if the feasibility judgment result is negative, until the feasibility judgment result is positive;

[0047] Collision determination module: configured to, based on a positive feasibility determination result, perform a collision determination on the local trajectory, wherein the collision determination is to determine whether the local trajectory is within the collision zone of the new obstacle, the collision zone of the new obstacle being calculated using the RVO algorithm; and, based on a positive collision determination result, return to and loop through the steps of "planning the local trajectory using the TEB algorithm," "determining the feasibility of the local trajectory," and "determining the collision of the local trajectory" until a negative collision determination result is returned.

[0048] Arrival judgment module: used to judge whether the unmanned intelligent vehicle has reached the target position based on the collision judgment result being negative; and based on the judgment result of whether the target position has been reached, it is used to return to and loop the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment", "local trajectory collision judgment" and "judging whether the unmanned intelligent vehicle has reached the target position" until the judgment result of reaching the target position is reached;

[0049] Completion module: used to complete path optimization based on the judgment result of reaching the target location.

[0050] In a specific embodiment of the present invention, the local planning module includes a first submodule and a second submodule;

[0051] The first submodule is used to convert the global path into initial trajectory points through the TEB algorithm;

[0052] The second submodule is used to complete the starting position posture to reach the target position posture under four target constraint functions, namely the path following and obstacle constraint function, the speed and acceleration constraint function, the non-holonomic kinematics constraint function and the fastest path constraint function.

[0053] Beneficial effects of the present invention:

[0054] The present invention provides a path optimization method and system that integrates the Transistor Electric Boundary Path (TEB) and Reverse Orbital Path (RVO) algorithms. In the method, after global planning is performed using the A-Star algorithm, the Transistor Electric Boundary Path (TEB) algorithm is then used to plan local trajectories. When planning the local trajectories, four objective constraint functions are added to the TEB algorithm to reduce turning points in the path and make the path smoother. Furthermore, while ensuring the trajectory as much as possible, the parameters required for the TEB algorithm are reduced, thereby reducing the computing resources occupied by path planning. The RVO algorithm is then used to perform collision judgment on the local trajectory obtained by the TEB algorithm, and a secondary constraint is imposed on the obstacle avoidance speed of the unmanned intelligent vehicle approaching dynamic obstacles, so that the unmanned intelligent vehicle can reasonably and efficiently resolve collision conflicts, making the trajectory of the unmanned intelligent vehicle tend to be smoother and the speed output more reasonable, thereby avoiding unknown obstacles in real time.

[0055] Other features and advantages of the present invention will be described in the following description, and in part will become apparent from the description, or will be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures pointed out in the description, claims and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0057] Figure 1 A flow chart of a path optimization method integrating TEB and RVO algorithms according to an embodiment of the present invention is shown;

[0058] Figure 2 The motion model of the four-wheel pendulum suspension unmanned intelligent vehicle according to an embodiment of the present invention is shown;

[0059] Figure 3 It shows one of the speed schematic diagrams of the four-wheel pendulum suspension unmanned intelligent vehicle according to an embodiment of the present invention;

[0060] Figure 4 It shows one of the speed schematic diagrams of the four-wheel pendulum suspension unmanned intelligent vehicle according to an embodiment of the present invention;

[0061] Figure 5 A schematic flow chart of the steps of determining the collision of a local trajectory according to an embodiment of the present invention is shown;

[0062] Figure 6 A framework diagram of a path optimization system integrating TEB and RVO algorithms according to an embodiment of the present invention is shown;

[0063] In the figure: simulation module 1; A-star algorithm module 2; local planning module 3; feasibility judgment module 4; collision judgment module 5; arrival judgment module 6; completion module 7. DETAILED DESCRIPTION

[0064] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0065] like Figure 1 As shown, a path optimization method integrating TEB and RVO algorithms according to an embodiment of the present invention includes:

[0066] Step S1: construct a mathematical model of the unmanned intelligent vehicle; establish a static map of the environment using a grid method, and obtain the current position and global target position of the unmanned intelligent vehicle on the static map of the environment;

[0067] Step S2: Obtain the global path of the unmanned intelligent vehicle through the A-star algorithm;

[0068] Step S3: planning a local trajectory for the global path using the TEB algorithm;

[0069] Step S4: Perform feasibility judgment on the local trajectory, wherein the feasibility judgment is to judge whether the local trajectory is feasible; if the feasibility judgment result is negative, return to and loop through the steps of "obtaining the global path using the A-star algorithm", "planning the local trajectory using the TEB algorithm", and "judging the feasibility of the local trajectory" until the feasibility judgment result is positive;

[0070] Step S5: If the feasibility determination result is yes, perform a collision determination on the local trajectory, wherein the collision determination is to determine whether the local trajectory is within the collision zone of the new obstacle, where the collision zone of the new obstacle is calculated using the RVO algorithm. If the collision determination result is yes, return to and loop through the steps of "planning the local trajectory using the TEB algorithm," "determining the feasibility of the local trajectory," and "determining the collision of the local trajectory" until the collision determination result is no.

[0071] Step S6: Based on the collision judgment result being negative, determine whether the unmanned intelligent vehicle has reached the target posture; based on the determination result of whether the target posture has been reached, return to and loop through the steps of "TEB algorithm planning local trajectory", "determining the feasibility of the local trajectory", "determining the collision of the local trajectory", and "determining whether the unmanned intelligent vehicle has reached the target position" until a determination result of reaching the target position is reached;

[0072] Step S7: Based on the result of the determination of reaching the target location, the path optimization is completed.

[0073] In this embodiment of the present invention, the unmanned intelligent vehicle is exemplarily studied using a four-wheel pendulum suspension unmanned intelligent vehicle as the research object, illustrating step S1 of the optimization method provided by the present invention. In other embodiments of the present invention, the unmanned intelligent vehicle can be other types of unmanned intelligent vehicles (such as pendulum suspension vehicles, independent suspension vehicles, and dependent suspension vehicles), and the mathematical model is constructed for this type of unmanned intelligent vehicle. The present invention does not specifically limit the construction process of the mathematical model of a specific type of unmanned intelligent vehicle.

[0074] The four-wheel pendulum suspension unmanned intelligent vehicle is mainly composed of detection modules such as laser radar, RGB depth camera, IMU (MPU6050), odometer, etc., control modules such as Raspberry Pi microcomputer, STM32 single-chip microcomputer, and drive modules such as motors with encoders. To simplify the kinematic mathematical model, the default distance between motors AB is equal to the distance between motors CD, which is H, and the distance between motors BC is equal to the distance between motors AD, which is D. In addition, the unmanned intelligent vehicle motion model is established without considering wheel idling, slipping, and the inconsistency between the geometric center and the center of mass of the intelligent vehicle. Its simplified structural diagram is referenced. Figure 2 Specifically, V1, V2, V3, and V4 are the speeds of motors A, B, C, and D, respectively. When the speeds of the four driving wheel motors of the smart car are different, a rotation speed ω will be generated around the rotation center according to the speed synthesis. c , realize the steering. In addition, it needs to be further explained that, refer to Figure 3 When the smart car encounters sliding friction and turns around the rotation center O, the two wheels in the same horizontal direction have the same speed. The four-wheel drive model can be equivalent to the two-wheel drive model. Figure 4 , thus simplifying the forward kinematics formula and the inverse kinematics formula;

[0075] The forward kinematics formula is shown in formula (1):

[0076]

[0077] In formula (1), ω c is the rotation speed, V x is the linear velocity of the smart car on the X axis, V r and V lDenote the longitudinal component velocities of the right and left wheels respectively, and D2 is the distance between the wheels of the equivalent two-wheel model.

[0078] The inverse kinematics formula is shown in formula (2):

[0079]

[0080] In formula (2), V r and V l Denote the longitudinal component velocities of the right and left wheels respectively, and D2 is the distance between the wheels of the equivalent two-wheel model.

[0081] Based on the forward kinematics formula and the inverse kinematics formula, a mathematical model of a four-wheel pendulum suspension unmanned intelligent vehicle is constructed;

[0082] Based on the configuration of a four-wheel pendulum suspension unmanned intelligent vehicle, in order to improve the effectiveness and safety of the fusion algorithm proposed in this invention in the path planning of the unmanned intelligent vehicle, a Raspberry Pi unmanned intelligent vehicle equipped with an LD14P laser radar was used as the experimental platform. Using the Linux operating system, a corridor with a relatively open space and fewer uncertain factors such as passers-by was used as the experimental environment. By placing obstacles and other methods to simulate the outdoor environment, an experimental map was constructed.

[0083] Since sidewalks are mostly narrow planes, the Gmapping algorithm can ensure high-precision mapping with low computing power for the small scene construction selected in the experiment. Therefore, in the embodiment of the present invention, the Gmapping algorithm is used as an example to create a rasterized map, and the Gmapping algorithm adds odom type data obtained by the IMU sensor, effectively using odometer information, so that the unmanned intelligent vehicle can accurately locate its own position and posture on the map while building the map. The frequency of use of lidar is lower but the robustness is better, achieving good mapping effect.

[0084] The specific operation of "obtaining the current position and global target position of the unmanned intelligent vehicle on the static map of the environment" is divided into four steps:

[0085] i. Turn on the unmanned smart car's Wi-Fi and connect to it using the Ubuntu virtual machine.

[0086] ii. Download the Gmapping algorithm related function package and prepare the relevant files and parameters;

[0087] iii. Use roslaunch to run the mapping program and check whether the relevant nodes are started normally;

[0088] iv. Use the keyboard to control the car to build a map around the experimental environment, and further adjust the parameters. Repeat multiple times and select the best parameters to use.

[0089] The posture includes the position information and speed direction of the unmanned intelligent vehicle (in the four-wheel pendulum suspension unmanned intelligent vehicle, considering v in formula (4) i direction) information.

[0090] In the static map construction process for a four-wheel pendulum suspension autonomous vehicle, the actual conditions of the simulation scenario must be considered to improve the accuracy of the optimization method. In this example, the default parameter values ​​for the costmap resolution and the autonomous vehicle's expansion coefficient are modified as follows:

[0091] Considering that this invention models within a small-scale scenario, the cost map resolution is set to 0.1 square meters per cell (the radius of an adult human body is approximately 0.2 meters). This is because the cost map resolution directly affects the efficiency and accuracy of the A-star global planning algorithm. When the map resolution is low, the resulting map grid size is large and the number of grids is small, resulting in a large gap between the found path and the actual optimal path. When the map resolution is high, although a better route can be obtained, the computing power, time, and amount of calculation consumed will become very large.

[0092] Obstacles are usually expanded by a certain coefficient. This time, the expansion coefficient of the unmanned intelligent vehicle was set to 0.25 (0.5 times the default setting) to facilitate the construction of the obstacle shape on the grid map and the calculation of the path planning algorithm, and to avoid reducing the error of the lidar or IMU sensor when detecting obstacles.

[0093] In step S2, the global path of the unmanned intelligent vehicle is obtained by the A-star algorithm, that is, based on the static map of the environment and the current position and global target position of the unmanned intelligent vehicle on the static map of the environment, the global path of the unmanned intelligent vehicle is obtained by the A-star algorithm. The A-star algorithm calculation involved in this process is a traditional A-star algorithm calculation, which is common knowledge for people in this technical field. Here, the embodiment of the present invention will not be described in detail.

[0094] In step S3, the TEB algorithm is used to plan a local trajectory for the global path, specifically including:

[0095] a. Convert the global path into initial trajectory points through the TEB algorithm;

[0096] b. Based on the initial trajectory point, complete the starting position and posture to reach the target position and posture under four objective constraint functions, the four objective constraint functions including path following and obstacle constraint function, velocity and acceleration constraint function, non-holonomic kinematic constraint function and fastest path constraint function.

[0097] In step b, the path following and obstacle constraint function is expressed as formula (3):

[0098]

[0099] In formula (3), f path and f ob is the penalty function, x Γ is the boundary, ε is the offset factor, S is the scaling factor, n is the order, d min,j is the independent variable, indicating the distance between the path and the obstacle, r pmax is the maximum distance the trajectory deviates from the path, r omin Indicates the minimum distance between the trajectory and the obstacle;

[0100] The expression of the velocity and acceleration constraint function is shown in formula (4):

[0101]

[0102] In formula (4), v i is the linear velocity of the unmanned intelligent vehicle at time i, ΔT i is the time interval between adjacent pose points, (x i ,y i ) is the coordinate of the unmanned intelligent vehicle at time i, ω i is the angular velocity of the unmanned intelligent vehicle at time i, β i is the orientation angle of the unmanned intelligent vehicle at time i, a i is the linear acceleration of the unmanned intelligent vehicle at time i;

[0103] The expression of the nonholonomic kinematic constraint function is shown in formula (5):

[0104]

[0105] In formula (5), f k (X i ,X i+1 ) is the objective function, d i,i+1 is the direction vector, β i is the orientation angle of the unmanned intelligent vehicle at time i;

[0106] The expression of the fastest path constraint function is shown in formula (6).

[0107]

[0108] In this embodiment of the present invention, the TEB algorithm is improved by introducing four objective constraint functions when planning local trajectories for a global path. This reduces turning points in the path and makes the path smoother. Furthermore, while minimizing the trajectory, the parameters required by the TEB algorithm are reduced, thereby reducing the computational resources used in path planning.

[0109] Moreover, when the TEB algorithm plans the local trajectory for the global path, the expansion coefficient of the unmanned intelligent vehicle is set to 0.25.

[0110] After step S3, the local trajectory is obtained, and the feasibility of the obtained trajectory is judged, that is, whether the obtained trajectory is feasible, that is, step S4 is performed. If the feasibility judgment result is no, the process returns to and loops through the steps of "obtaining the global path using the A-star algorithm", "planning the local trajectory using the TEB algorithm", and "judging the feasibility of the local trajectory" (i.e., repeating steps S2, S3, and the feasibility judgment step) until the feasibility judgment result is yes.

[0111] like Figure 5 As shown, if the feasibility judgment result is yes, the collision judgment of the local trajectory is performed. The collision judgment is to determine whether the local trajectory is within the collision zone of the new obstacle, which specifically includes:

[0112] a. If the feasibility check result is yes, determine whether a new obstacle has appeared in the local map;

[0113] b. Based on the result of determining whether a new obstacle has appeared, choose whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle;

[0114] c. Determine whether the local trajectory is within the collision zone of the new obstacle.

[0115] In step b, based on the result of determining whether a new obstacle has appeared, the decision is made whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle, including:

[0116] If a new obstacle is detected, the collision area of ​​the new obstacle is calculated using the RVO algorithm, i.e., the RVO algorithm is activated to perform secondary optimization on the path. The RVO algorithm in this step is a traditional RVO algorithm, which is well known to those skilled in the art and will not be described in detail in this embodiment of the present invention.

[0117] If there is no new obstacle judgment result, the steps of "the unmanned intelligent vehicle moves toward the target posture" and "determine whether the unmanned intelligent vehicle has reached the target posture" are performed in sequence.

[0118] In step c, if the collision judgment result is yes, then return to and loop the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment" and "local trajectory collision judgment" (i.e., return to and loop steps S3, S4 and "local trajectory collision judgment") until the collision judgment result is no. That is, after the TEB algorithm outputs the speed, determine whether the speed direction belongs to the collision area of ​​the RVO algorithm. If so, cancel the output of the speed, and select the loop of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment" and "local trajectory collision judgment" until a suboptimal speed is found, and the speed direction of the suboptimal speed meets the condition that the collision judgment result is no.

[0119] If the collision determination result is negative, then determine whether the unmanned intelligent vehicle has reached the target position, i.e., proceed to step S6;

[0120] From the subdivision steps of step S5, it can be seen that if a new obstacle appears in the local map, the RVO algorithm is activated to perform secondary optimization of the path. This is because the TEB algorithm itself is a combination optimization of the set constraint functions. Therefore, when the unmanned intelligent vehicle is in a complex dynamic environment or close to a dynamic obstacle and the computing power is limited, if the constraint function uses a smaller penalty factor, the unmanned intelligent vehicle will break through the constraint conditions with smaller weights, causing its own speed to jump repeatedly, or even collide with obstacles, etc., which will lead to a series of consequences that lead to path planning failure. At this time, starting the RVO algorithm to perform secondary optimization of the path can avoid the aforementioned consequences of repeated jumps in its own speed and collisions with obstacles. This is because the RVO algorithm, as a dynamic obstacle avoidance method, completely abandons the idea of ​​speed directions that may cause collisions, which just makes up for the shortcomings of the TEB algorithm. The disadvantage of the RVO algorithm is that it lacks consideration of the dynamic constraints of the unmanned intelligent vehicle, and sometimes may produce new speed directions that cannot be reached, but this is just compensated by the TEB algorithm. Furthermore, in an embodiment of the present invention, the TEB algorithm is first used to plan a local path, and then the RVO algorithm is optimized for the planned local path in a complex dynamic environment where new obstacles appear. This effectively integrates the TEB algorithm and the RVO algorithm, and realizes the advantages and disadvantages of the two algorithms, so that the trajectory of the unmanned intelligent vehicle tends to be smoother and the speed output is more reasonable, thereby avoiding unknown obstacles in real time.

[0121] After passing step S5, that is, the collision determination result is negative, it is determined whether the unmanned intelligent vehicle has reached the target posture, that is, step 6 is performed;

[0122] If the result of the judgment on whether the unmanned intelligent vehicle has reached the target position is correct, the process returns to and loops through the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment", "local trajectory collision judgment", and "judging whether the unmanned intelligent vehicle has reached the target position" until the result of the judgment on whether the unmanned intelligent vehicle has reached the target position is correct, i.e., steps S3-S5 are repeated until the path optimization is completed;

[0123] If the judgment result of whether the intelligent unmanned vehicle reaches the target position is obtained, the path optimization is completed, that is, step S7 is performed.

[0124] As shown in Figure 6 The path optimization system fusing the TEB and RVO algorithms according to the embodiment of the application comprises:

[0125] The simulation module 1 is configured to construct a mathematical model of the intelligent unmanned vehicle, and to establish an environment static map by a grid method and obtain a current pose and a global target pose of the intelligent unmanned vehicle on the environment static map.

[0126] The A-star algorithm module 2 is configured to obtain a global path of the intelligent unmanned vehicle by an A-star algorithm.

[0127] The local planning module 3 is configured to plan a local trajectory for the global path by a TEB algorithm.

[0128] The realization judgment module 4 is configured to perform realization judgment on the local trajectory, wherein the realization judgment is to judge whether the local trajectory can be realized, and based on the realization judgment result being no, to return and loop the steps of "obtaining the global path by the A-star algorithm", "planning the local trajectory by the TEB algorithm" and "realization judgment on the local trajectory" until the realization judgment result is yes.

[0129] The collision judgment module 5 is configured to, based on the realization judgment result being yes, perform collision judgment on the local trajectory, wherein the collision judgment is to judge whether the local trajectory is in a collision area of a new obstacle, and the collision area of the new obstacle is obtained by an RVO algorithm, and based on the collision judgment result being yes, to return and loop the steps of "planning the local trajectory by the TEB algorithm", "realization judgment on the local trajectory" and "collision judgment on the local trajectory" until the collision judgment result is no.

[0130] The arrival judgment module 6 is configured to, based on the collision judgment result being no, judge whether the intelligent unmanned vehicle reaches the target pose, and based on the judgment result of whether the intelligent unmanned vehicle reaches the target pose being no, to return and loop the steps of "planning the local trajectory by the TEB algorithm", "realization judgment on the local trajectory", "collision judgment on the local trajectory" and "judging whether the intelligent unmanned vehicle reaches the target position" until the judgment result of whether the intelligent unmanned vehicle reaches the target position is obtained.

[0131] The completion module 7 is configured to, based on the judgment result of whether the intelligent unmanned vehicle reaches the target position, complete the path optimization.

[0132] In the embodiment of the application, the local planning module 3 comprises a first sub-module and a second sub-module.

[0133] The first sub-module is configured to convert the global path into initial trajectory points by using a TEB algorithm.

[0134] The second sub-module is configured to reach the target position pose from the start position pose under four target constraint functions, including a path following and obstacle constraint function, a velocity and acceleration constraint function, a nonholonomic constraint function, and a fastest path constraint function.

[0135] Although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those skilled in the art that the technical solutions recorded in the foregoing embodiments can be modified, or some of the technical features can be replaced by equivalents, without departing from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A path optimization method integrating TEB and RVO algorithms, characterized in that: include: Construct a mathematical model for unmanned intelligent vehicles; Establish a static map of the environment through the grid method, and obtain the current position and global target position of the unmanned intelligent vehicle on the static map of the environment; Obtain the global path of the unmanned intelligent vehicle through the A-star algorithm; The local trajectory is planned for the global path through the TEB algorithm; Performing feasibility judgment on the local trajectory, wherein the feasibility judgment is to judge whether the local trajectory is feasible; If the feasibility check result is negative, return to and loop through the steps of "obtaining the global path using the A-star algorithm," "planning the local trajectory using the TEB algorithm," and "judging the feasibility of the local trajectory" until the feasibility check result is positive. If the feasibility determination result is yes, a collision determination is performed on the local trajectory, wherein the collision determination is to determine whether the local trajectory is within a collision zone of the new obstacle, where the collision zone of the new obstacle is calculated using the RVO algorithm. If the collision determination result is yes, return to and loop through the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility determination", and "local trajectory collision determination" until the collision determination result is no. If the collision judgment result is negative, determine whether the unmanned intelligent vehicle has reached the target position; Based on the result of whether the target position has been reached, the process returns to and loops through the steps of "TEB algorithm planning local trajectory", "determining the feasibility of the local trajectory", "determining the collision of the local trajectory", and "determining whether the unmanned intelligent vehicle has reached the target position" until the target position is reached. Based on the judgment result of reaching the target location, the path optimization is completed.

2. A path optimization method integrating TEB and RVO algorithms according to claim 1, characterized in that: The posture includes the position information and speed direction information of the unmanned intelligent vehicle.

3. The path optimization method integrating TEB and RVO algorithms according to claim 1, characterized in that: When the unmanned intelligent vehicle is a four-wheel pendulum suspension unmanned intelligent vehicle, during the process of establishing the static map of the environment, the default parameter values ​​of the cost map resolution and the expansion coefficient of the unmanned intelligent vehicle are modified.

4. The path optimization method integrating TEB and RVO algorithms according to claim 1, characterized in that: The local trajectory planning of the global path by the TEB algorithm includes: The global path is converted into initial trajectory points through the TEB algorithm; Based on the initial trajectory point, the starting position and posture are completed to reach the target position and posture under four target constraint functions, including path following and obstacle constraint function, speed and acceleration constraint function, non-holonomic kinematic constraint function and fastest path constraint function.

5. The path optimization method integrating TEB and RVO algorithms according to claim 4, characterized in that: The expressions of the path following and obstacle constraint functions are as follows: Among them, f path and f ob is the penalty function, x Γ is the boundary, ε is the offset factor, S is the scaling factor, n is the order, d min,j is the independent variable, indicating the distance between the path and the obstacle, r pmax is the maximum distance the trajectory deviates from the path, r omin Indicates the minimum distance between the trajectory and the obstacle; The expressions of the velocity and acceleration constraint functions are as follows: Among them, v i is the linear velocity of the unmanned intelligent vehicle at time i, ΔT i is the time interval between adjacent pose points, (x i ,y i ) is the coordinate of the unmanned intelligent vehicle at time i, ω i is the angular velocity of the unmanned intelligent vehicle at time i, β i is the orientation angle of the unmanned intelligent vehicle at time i, a i is the linear acceleration of the unmanned intelligent vehicle at time i.

6. The path optimization method integrating TEB and RVO algorithms according to claim 4, characterized in that: The expression of the nonholonomic kinematic constraint function is as follows: Among them, f k (X i ,X i+1 ) is the objective function, d i,i+1 is the direction vector, (x i ,y i ) is the coordinate of the unmanned intelligent vehicle at time i, β i is the orientation angle of the unmanned intelligent vehicle at time i; The expression of the fastest path constraint function is as follows: Where, ΔT i is the time interval between adjacent pose points.

7. A path optimization method integrating TEB and RVO algorithms according to any one of claims 1 to 6, characterized in that: The collision determination of the local trajectory is performed based on the feasibility determination result being yes, including: If the feasibility judgment result is yes, check whether new obstacles appear in the local map; Based on the judgment result of whether a new obstacle appears, choose whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle; Determine whether the local trajectory is within the collision zone of the new obstacle.

8. The path optimization method integrating TEB and RVO algorithms according to claim 7, characterized in that: The step of selecting whether to use the RVO algorithm to calculate the collision area of ​​the new obstacle based on the result of determining whether a new obstacle has appeared includes: Based on the judgment result of the new obstacle, the collision area of ​​the new obstacle is calculated using the RVO algorithm; Based on the judgment result that no new obstacles appear, the steps of "the unmanned intelligent vehicle moves toward the target posture" and "determine whether the unmanned intelligent vehicle has reached the target posture" are performed in sequence.

9. A path optimization system integrating TEB and RVO algorithms, characterized in that: include: Simulation module: used to build a mathematical model of unmanned intelligent vehicles; It is also used to establish a static map of the environment through a grid method, and obtain the current position and global target position of the unmanned intelligent vehicle on the static map of the environment; A-star algorithm module: used to obtain the global path of the unmanned intelligent vehicle through the A-star algorithm; Planning local module: used to plan local trajectories for the global path through the TEB algorithm; Feasibility judgment module: used to judge the feasibility of the local trajectory, wherein the feasibility judgment is to determine whether the local trajectory is feasible; and used to return to and loop the "A-star algorithm to obtain the global path" and "TEB algorithm to plan the local trajectory" and "feasibility judgment of the local trajectory" steps if the feasibility judgment result is negative, until the feasibility judgment result is positive; Collision determination module: Based on a positive feasibility determination, the module performs a collision determination on the local trajectory. The collision determination involves determining whether the local trajectory is within the collision zone of the new obstacle, as calculated using the RVO algorithm. Based on a positive collision determination, the module returns to and loops through the steps of "planning the local trajectory using the TEB algorithm," "determining the feasibility of the local trajectory," and "determining the collision of the local trajectory" until a negative collision determination is returned. Arrival judgment module: used to judge whether the unmanned intelligent vehicle has reached the target position based on the collision judgment result being negative; and based on the judgment result of whether the target position has been reached, it is used to return to and loop the steps of "TEB algorithm planning local trajectory", "local trajectory feasibility judgment", "local trajectory collision judgment" and "determining whether the unmanned intelligent vehicle has reached the target position" until the judgment result of reaching the target position is reached; Completion module: used to complete path optimization based on the judgment result of reaching the target location.

10. The path optimization system integrating TEB and RVO algorithms according to claim 9, characterized in that: The planning local module includes a first submodule and a second submodule; The first submodule is used to convert the global path into initial trajectory points through the TEB algorithm; The second submodule is used to complete the starting position posture to reach the target position posture under four target constraint functions, namely the path following and obstacle constraint function, the speed and acceleration constraint function, the non-holonomic kinematics constraint function and the fastest path constraint function.

Images