Moving body trajectory planning method and device, electronic equipment and storage medium

Abstract

The embodiment of the invention discloses a moving body track planning method and device, electronic equipment and a storage medium, and relates to the technical field of track planning, and the method comprises the steps: carrying out the geometric boundary expansion and minimum distance detection of a moving body, and obtaining the environment complexity of a starting point and an end point of the moving body, the method comprises the steps of obtaining an initial trajectory and constructing kinematic constraints based on a mixed A * algorithm of a bidirectional search strategy, constructing a safety corridor based on the initial trajectory to obtain obstacle avoidance constraints, calculating environment complexity of each trajectory point according to the safety corridor, classifying the trajectory points to obtain sampling time constraints, and obtaining final constraints according to the constraints. Using the initial trajectory as an initial solution to obtain a planned trajectory, performing transverse and longitudinal decoupling on the planned trajectory to respectively obtain transverse and longitudinal state equations, and performing tracking control on the moving body by adopting a model predictive control (MPC) algorithm. According to the method, the problems of low calculation efficiency, infeasible trajectory planning result and poor optimization process convergence in the prior art are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory planning, and in particular to a method, device, electronic device and storage medium for planning a trajectory of a moving object. Background Art

[0002] With the rapid development of autonomous driving technology, trajectory planning and control of moving bodies in complex scenarios has always been a challenging task. Existing trajectory planning methods often suffer from low computational efficiency in complex environments, especially those with irregular, non-convex obstacles. Furthermore, trajectory planning results are often infeasible in complex environments, and convergence of the optimization process is difficult to guarantee.

[0003] Furthermore, traditional optimization-based trajectory planning methods fail to adequately consider environmental complexity during the planning process. They often overlook key factors such as obstacle distribution and spatial confinement, significantly reducing the success rate of parking in narrow spaces.

[0004] Moreover, most existing methods adopt a decoupled planning approach for trajectory and speed. Although this approach simplifies the problem to a certain extent, it greatly reduces the controllability of the final planned trajectory in actual control, making it difficult to meet the high precision and high stability requirements in complex parking scenarios.

[0005] Therefore, there is an urgent need for a moving body trajectory planning method that can fully consider the complexity of the environment, improve the success rate of trajectory planning and control, and enhance the controllability of the trajectory. Summary of the Invention

[0006] The embodiments of the present invention provide a method for trajectory planning of a moving object to solve the problems of low computational efficiency, infeasible trajectory planning results, and poor convergence of the optimization process in the existing technology when facing complex scenes. The technical solution is as follows: According to one aspect of the present invention, a method for planning a trajectory of a moving body includes: obtaining sensory information of a starting point and an end point in a planning task, performing geometric boundary expansion and minimum distance detection on the moving body based on the sensory information to obtain the environmental complexity of the starting point and the end point of the moving body; obtaining an initial trajectory based on a hybrid A* algorithm of the environmental complexity and a two-way search strategy, and performing kinematic modeling on the moving body to construct kinematic constraints; constructing a safety corridor by circularly approximating the moving body based on the initial trajectory, and obtaining an obstacle avoidance constraint for the planning task by constraining the coordinates of the center of the circle; calculating the environmental complexity of each trajectory point in the initial trajectory based on the safety corridor, and classifying the trajectory points by sampling time based on the environmental complexity to obtain a sampling time constraint; obtaining a final constraint based on the kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness, and comfort optimization constraints, and using the initial trajectory as an initial solution to obtain a planned trajectory according to the interior point method; decoupling the planned trajectory laterally and longitudinally to obtain lateral and longitudinal state equations, respectively, and using a model predictive control (MPC) algorithm to track and control the moving body according to the state equations.

[0007] In one embodiment, acquiring perception information of a starting point and an end point in a planning task, and performing geometric boundary expansion and minimum distance detection on a moving body based on the perception information to obtain environmental complexity at the starting point and the end point of the moving body are achieved by the following steps: acquiring perception information of the planning task; the planning task includes the two-dimensional coordinates and heading angle of the moving body at the starting point and the end point; expanding the geometric model of the moving body along the expansion direction according to the actual size of the moving body and the map resolution to obtain perception areas of the moving body at the starting point and the end point; the expansion direction includes forward, backward, left, and right; obtaining the minimum distance between the moving body and obstacles in the expansion direction within the perception area in an iterative manner according to the map resolution, and setting a distance threshold; setting a weight and environmental factor of the moving body in the expansion direction according to the kinematics of the moving body and the distance threshold, and setting a nonlinear adjustment parameter and an amplification factor; the amplification factor is used to enhance the environmental complexity of the corresponding expansion direction when obstacles are dense; calculating an environmental complexity contribution value of the moving body in the expansion direction according to the weight, environmental factor, minimum distance, adjustment parameter, and amplification factor, and performing a weighted summation of the environmental complexity contribution values ​​to obtain the environmental complexity of the starting point and the end point of the moving body.

[0008] In one embodiment, the formula for calculating the environmental complexity contribution value in the expansion direction includes: ; in, Indicates the expansion direction, Indicates direction The weight of Indicates an environmental factor with a value of 0 or 1, Indicates direction The minimum distance, represents the adjustment parameter, Represents the amplification factor.

[0009] In one embodiment, a hybrid A* algorithm based on the environmental complexity and a bidirectional search strategy obtains an initial trajectory, and kinematic modeling of the moving body is performed to construct kinematic constraints through the following steps: when the environmental complexity of the starting point is greater than the environmental complexity of the end point, a forward hybrid A* algorithm is used from the starting point; when the environmental complexity of the starting point is less than the environmental complexity of the end point, a reverse hybrid A* algorithm is used from the end point to obtain the initial trajectory; kinematic modeling of the moving body is performed to determine state variables, a continuous state equation of the moving body is constructed according to the state variables, the state equation is discretized using a cubic Hermit algorithm, and then the initial trajectory is optimized in space-time using the state equation; the state variables include the two-dimensional coordinates, speed, steering wheel angle and heading angle of the moving body.

[0010] In one embodiment, a safety corridor is constructed by approximating the moving body by a circle based on the initial trajectory, and the obstacle avoidance constraint of the planning task is obtained by constraining the center coordinates of the circle through the following steps: three circles of the same size are used to cover the moving body, and for the three circles corresponding to each trajectory point in the initial trajectory, an outer circle is iteratively constructed in a direction parallel to the world coordinate system and expanded in sequence to obtain the boundary of the safety corridor; a boundary constraint is set on the center coordinates of the circle according to the boundary of the safety corridor to obtain the obstacle avoidance constraint of the planning task, and the weight of the obstacle avoidance constraint is set to 109.

[0011] In one embodiment, the environmental complexity of each trajectory point in the initial trajectory is calculated based on the safety corridor, and the sampling time of the trajectory points is classified according to the environmental complexity to obtain the sampling time constraint by the following steps: the environmental complexity of each trajectory point in the initial trajectory is calculated based on the safety corridor; the environmental complexity reflects the narrowness of the space around the trajectory point of the moving body or the density of obstacles; the trajectory points are divided into wide points and narrow points according to the environmental complexity, the maximum sampling time of the wide point and the minimum sampling time of the narrow point are calculated as reference values, and the square sum of the errors between the current sampling time and the reference value is calculated and multiplied by a weight as the sampling time constraint.

[0012] In one embodiment, the planned trajectory is decoupled in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, and the model predictive control algorithm MPC is used to track and control the moving body according to the state equations through the following steps: the planned trajectory is decoupled in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, a tracking trajectory point is selected from the planned trajectory according to the current state of the moving body, and a lateral error and a longitudinal error are calculated for the tracking trajectory point according to the state equation to obtain a lateral error and a longitudinal error; the moving body is laterally controlled by a lateral MPC controller according to the lateral error, and the moving body is longitudinally controlled by a longitudinal MPC controller according to the longitudinal error, and the lateral error and the longitudinal error are fed back according to the operating state of the moving body and the state equation.

[0013] According to one aspect of the present invention, a moving body trajectory planning device includes: an environmental assessment module for obtaining perception information of the starting point and end point in the planning task, and performing geometric boundary expansion and minimum distance detection on the moving body according to the perception information to obtain the environmental complexity of the starting point and end point of the moving body; an initial trajectory search module for obtaining an initial trajectory based on a hybrid A* algorithm of the environmental complexity and a two-way search strategy, and performing kinematic modeling on the moving body to construct kinematic constraints; an obstacle avoidance constraint construction module for constructing a safe corridor by approximating the moving body through a circle based on the initial trajectory, and obtaining the obstacle avoidance constraint of the planning task by constraining the coordinates of the center of the circle. obstacle constraint; a sampling time constraint construction module, used to calculate the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor, and classify the sampling time of the trajectory points according to the environmental complexity to obtain the sampling time constraint; a trajectory planning module, used to obtain the final constraint according to the kinematic constraint, obstacle avoidance constraint, sampling time constraint, trajectory smoothness and comfort optimization constraint, and use the initial trajectory as the initial solution to obtain the planned trajectory according to the interior point method; a tracking control module, used to decouple the planned trajectory in the lateral and longitudinal directions to obtain the lateral and longitudinal state equations respectively, and use the model predictive control MPC algorithm to track and control the moving body according to the state equations.

[0014] According to one aspect of the present invention, an electronic device includes at least one processor and at least one memory, wherein the memory stores computer-readable instructions; the computer-readable instructions are executed by one or more of the processors, so that the electronic device implements the moving body trajectory planning method as described above.

[0015] According to one aspect of the present invention, a storage medium stores computer-readable instructions thereon, wherein the computer-readable instructions are executed by one or more processors to implement the moving body trajectory planning method as described above.

[0016] The beneficial effects brought about by the technical solution provided by the present invention are: In the above technical solution, the present invention first obtains the perception information of the starting point and the end point in the planning task, performs geometric boundary expansion and minimum distance detection on the moving body, thereby accurately evaluating the complexity of the environment. On this basis, a hybrid A* algorithm based on environmental complexity and a two-way search strategy is used to obtain the initial trajectory from the starting point and the end point, and kinematic modeling is performed on the moving body to construct kinematic constraints. Then, based on the initial trajectory, a safe corridor is constructed by circularly approximating the moving body to obtain obstacle avoidance constraints. At the same time, the environmental complexity of each trajectory point in the initial trajectory is calculated according to the safe corridor, and the trajectory points are classified by sampling time to obtain sampling time constraints. Finally, the comprehensive operation Kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints are adopted. The planned trajectory is obtained using the interior point method, and the planned trajectory is decoupled in the lateral and longitudinal directions to obtain the lateral and longitudinal state equations respectively. The model predictive control MPC algorithm is used to track and control the moving body, thereby achieving precise control of the moving body. This not only improves the success rate of the moving body control, but also significantly enhances the controllability of the trajectory, so that the moving body can complete the control task more safely and stably in complex scenarios, thereby effectively solving the problems of low computational efficiency, infeasible trajectory planning results and poor convergence of the optimization process when existing technologies face complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort.

[0018] Figure 1 is a flow chart showing a method for planning a trajectory of a moving object according to an exemplary embodiment; Figure 2 It is a flowchart of the complexity of the computing environment in an application scenario; Figure 3 yes Figure 2 Schematic diagram of building a safety corridor in the corresponding application scenario; Figure 4 yes Figure 2 Flow chart of tracking control in corresponding application scenarios; Figure 5 is a block diagram of a moving body trajectory planning device according to an exemplary embodiment; Figure 6 is a hardware structure diagram of an electronic device according to an exemplary embodiment; Figure 7 It is a block diagram of an electronic device according to an exemplary embodiment. DETAILED DESCRIPTION

[0019] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended only to explain the present invention and are not to be construed as limiting the present invention.

[0020] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of the present disclosure refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or wireless couplings. The term "and / or" used herein includes all or any units and all combinations of one or more associated listed items.

[0021] The present invention provides a method for moving body trajectory planning. By considering environmental complexity, a two-way search strategy, safe corridor construction, and MPC tracking control, this method not only improves the success rate of moving body control but also significantly enhances the controllability of the trajectory. This method addresses the problems of low computational efficiency, infeasible trajectory planning results, and poor convergence of the optimization process in complex scenarios encountered by existing technologies. The method is suitable for use in a moving body trajectory planning device, which can be an electronic device. The moving body trajectory planning method in embodiments of the present invention can be applied to a variety of scenarios, such as autonomous parking and task scheduling for mobile robots.

[0022] See also Figure 1 , an embodiment of the present invention provides a moving body trajectory planning method, which is applicable to electronic equipment.

[0023] In the following method embodiments, for ease of description, the execution subject of each step of the method is taken as an electronic device as an example for illustration, but this does not constitute a specific limitation.

[0024] like Figure 1 As shown, the method may include the following steps: Step 110 , obtaining the perception information of the starting point and the end point in the planning task, performing geometric boundary expansion and minimum distance detection on the moving body based on the perception information to obtain the environmental complexity of the starting point and the end point of the moving body.

[0025] In one possible implementation, perception information for the planning task is obtained, and the geometric model of the moving body is expanded along the expansion direction according to the actual size of the moving body and the map resolution to obtain the perception area of ​​the moving body at the starting point and the end point. The minimum distance between the moving body and the obstacle in the expansion direction is obtained within the perception area in an iterative manner according to the map resolution, and a distance threshold is set.

[0026] Furthermore, the weight and environmental factor of the moving body in the expansion direction are set respectively according to the kinematics of the moving body and the distance threshold, and the nonlinear adjustment parameter and the amplification factor are set. The environmental complexity contribution value of the moving body in the expansion direction is calculated according to the weight, environmental factor, minimum distance, adjustment parameter and amplification factor, and the environmental complexity contribution value is weightedly summed to obtain the environmental complexity of the starting point and end point of the moving body.

[0027] Among them, the planning task includes the two-dimensional coordinates and heading angle of the moving body at the starting point and the end point. The expansion directions include forward, backward, left and right. The amplification factor is used to enhance the environmental complexity of the corresponding expansion direction when obstacles are dense.

[0028] In one possible implementation, the formula for calculating the environmental complexity contribution value in the expansion direction includes: ; in, Indicates the expansion direction, Indicates direction The weight of Indicates an environmental factor with a value of 0 or 1, Indicates direction The minimum distance, represents the adjustment parameter, Represents the amplification factor.

[0029] In the above process, the embodiment of the present invention can more accurately evaluate the movement space of the moving body in different directions by expanding the boundaries, and can more comprehensively consider the potential collision risk between the moving body and obstacles; by detecting the minimum distance between the moving body and obstacles, the narrowness and congestion of the environment can be intuitively understood, which is a key step in quantifying the complexity of the environment; by calculating the complexity of the environment, clear guidance is provided for subsequent path search and optimization, which is a process of converting perceptual information into quantifiable indicators.

[0030] Step 120 : A hybrid A* algorithm based on the environment complexity and the bidirectional search strategy is used to obtain an initial trajectory, and kinematic modeling is performed on the moving body to construct kinematic constraints.

[0031] In one possible implementation, when the environmental complexity of the starting point is greater than the environmental complexity of the end point, the forward hybrid A* algorithm is used from the starting point; when the environmental complexity of the starting point is less than the environmental complexity of the end point, the reverse hybrid A* algorithm is used from the end point to obtain the initial trajectory.

[0032] Furthermore, kinematic modeling is performed on the moving body to determine the state variables, and a continuous state equation of the moving body is constructed based on the state variables. The state equation is discretized using the cubic Hermit algorithm and then the initial trajectory is optimized using the state equation in a joint spatiotemporal manner.

[0033] The state variables include the two-dimensional coordinates, speed, steering wheel angle and heading angle of the moving body.

[0034] Furthermore, the embodiment of the present invention also designs a cost function including steering penalty, steering change penalty, driving distance penalty, movement direction change penalty and reversing penalty, which helps guide the algorithm to select a smoother and safer path, and quickly connects the two states obtained by forward and reverse search through the Reeds-Shepp curve to obtain a preliminary rough trajectory.

[0035] In the above process, the embodiment of the present invention adopts a hybrid A* algorithm that combines the advantages of heuristic search and grid search, which can quickly find feasible paths in complex environments; on the basis of the traditional hybrid A* algorithm, the environmental complexity information is integrated into the heuristic function, so that during the search process the algorithm tends to select paths with lower environmental complexity, thereby improving the feasibility and safety of the path; the use of a bidirectional search strategy can significantly speed up the search speed, especially when the distance between the starting point and the end point is far or the environment is complex.

[0036] In the above process, the embodiment of the present invention determines the state variables and control inputs that describe the motion of the moving body, establishes a discrete kinematic model based on the kinematic characteristics of the moving body, describes the state changes of the moving body in each discrete time step, then defines kinematic constraints based on factors such as road conditions and obstacle locations, and finally integrates these constraints into the mathematical problem of parking trajectory optimization as the constraint part of the optimization problem. During the optimization process, the algorithm will continuously adjust the trajectory of the moving body to meet these constraints.

[0037] Step 130 : constructing a safety corridor through a circular approximate moving body based on the initial trajectory, and obtaining obstacle avoidance constraints for the planned task by constraining the coordinates of the center of the circle.

[0038] In one possible implementation, three circles of the same size are used to cover the moving body. For each trajectory point in the initial trajectory, the three circles are iteratively constructed in a direction parallel to the world coordinate system and expanded in sequence to obtain the boundary of the safe corridor. According to the boundary of the safe corridor, the boundary constraint is set on the center coordinates of the circle to obtain the obstacle avoidance constraint of the planning task, and the weight of the obstacle avoidance constraint is set to 10. 9 .

[0039] In the above process, the embodiment of the present invention avoids collisions between the moving body and obstacles in the planning stage by constructing a safety corridor, thereby improving the feasibility of the trajectory; the safety corridor provides a safe spatial range for the moving body, ensuring that the moving body does not exceed this range during the movement process, thereby enhancing the safety of the movement process. After converting the safety corridor into a constraint condition in the optimization problem, it can be solved using an existing optimization algorithm to obtain a trajectory that meets the requirements.

[0040] Through this process, an optimized trajectory can be obtained that satisfies both the kinematic constraints of the moving body and the requirements of the mission scenario. This trajectory will serve as a reference trajectory for the subsequent tracking and control process, guiding the moving body to complete the task safely and efficiently.

[0041] Step 140 : Calculate the environmental complexity of each trajectory point in the initial trajectory based on the safety corridor, and classify the trajectory points by sampling time based on the environmental complexity to obtain sampling time constraints.

[0042] In one possible implementation, the environmental complexity of each trajectory point in the initial trajectory is calculated based on the safety corridor, and the trajectory points are divided into wide points and narrow points according to the environmental complexity. The maximum sampling time of the wide points and the minimum sampling time of the narrow points are calculated as reference values. The square sum of the errors between the current sampling time and the reference value is calculated and multiplied by the weight as the sampling time constraint.

[0043] Among them, the complexity of the environment reflects the narrowness of the space around the trajectory point of the moving object or the density of obstacles.

[0044] In the above process, an embodiment of the present invention uses a safe corridor to calculate the environmental complexity of each discrete trajectory point, and divides the trajectory points into two categories: wide and narrow according to the environmental complexity. These two categories of trajectory points correspond to two different sampling times, respectively. This sampling time is included in the optimization problem as an optimizable variable and directly acts on the kinematic constraints. At the same time, corresponding optimization objectives and constraints are designed for the sampling time, allowing the algorithm to dynamically adjust the sampling time according to the environmental complexity during the optimization process, thereby searching for trajectories more carefully in narrow spaces, thereby improving the success rate of planning and the quality of trajectories.

[0045] Step 150 , obtain the final constraints based on the kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints, and use the initial trajectory as the initial solution to obtain the planned trajectory according to the interior point method.

[0046] In the above process, the embodiment of the present invention integrates all the previous steps to construct a highly convex optimization problem. To solve this problem, a specific solver (such as the interior point method) is used, and the rough trajectory obtained by the front-end search is used as the initial solution for warm-up. Through the solution, a joint spatiotemporal control trajectory is finally obtained. This trajectory contains the state and control variables corresponding to each trajectory point.

[0047] In step 160 , the planned trajectory is decoupled in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, and the model predictive control (MPC) algorithm is used to track and control the moving body according to the state equations.

[0048] In one possible implementation, the planned trajectory is decoupled laterally and longitudinally to obtain the lateral and longitudinal state equations, respectively. Tracking trajectory points are selected from the planned trajectory according to the current state of the moving body. Lateral error calculations and longitudinal error calculations are performed on the tracking trajectory points according to the state equations to obtain the lateral error and longitudinal error.

[0049] Furthermore, the moving body is controlled laterally according to the lateral error by a lateral MPC controller, and the moving body is controlled longitudinally according to the longitudinal error by a longitudinal MPC controller, and the lateral error and the longitudinal error are fed back according to the operating state and state equation of the moving body.

[0050] In the above process, an embodiment of the present invention uses a model predictive control (MPC) method to track and control the planned parking trajectory. The trajectory tracking control is divided into two directions, lateral and longitudinal, for decoupled control. The longitudinal control is responsible for tracking the longitudinal target position and target speed of the planned trajectory, while the lateral control is responsible for tracking the lateral target position and target heading angle of the trajectory. By designing longitudinal and lateral error calculation methods and state equation models respectively, MPC is used to solve the optimal control input within each control cycle and apply it to the moving body system to achieve stable tracking control of the planned trajectory.

[0051] Through the above process, the present invention first obtains the perception information of the starting point and the end point in the planning task, performs geometric boundary expansion and minimum distance detection on the moving body, thereby accurately evaluating the complexity of the environment. On this basis, a hybrid A* algorithm based on environmental complexity and a two-way search strategy is used to obtain the initial trajectory from the starting point and the end point, and kinematic modeling is performed on the moving body to construct kinematic constraints. Then, based on the initial trajectory, a safe corridor is constructed by circularly approximating the moving body to obtain obstacle avoidance constraints. At the same time, the environmental complexity of each trajectory point in the initial trajectory is calculated according to the safe corridor, and the trajectory points are classified by sampling time to obtain sampling time constraints. Finally, the comprehensive operation Kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints are adopted. The planned trajectory is obtained using the interior point method, and the planned trajectory is decoupled in the lateral and longitudinal directions to obtain the lateral and longitudinal state equations respectively. The model predictive control MPC algorithm is used to track and control the moving body, thereby achieving precise control of the moving body. This not only improves the success rate of the moving body control, but also significantly enhances the controllability of the trajectory, so that the moving body can complete the control task more safely and stably in complex scenarios, thereby effectively solving the problems of low computational efficiency, infeasible trajectory planning results and poor convergence of the optimization process when existing technologies face complex scenarios.

[0052] In one application scenario, the embodiment of the present invention is applied to perform trajectory planning and control in a complex parking scenario. Figure 2 A schematic diagram of the process of calculating the complexity of the environment in a moving body trajectory planning method is shown. Figure 3 A schematic diagram of constructing a safe corridor in a moving body trajectory planning method is shown. Figure 4 A flow chart of tracking control in a moving body trajectory planning method is shown.

[0053] Specifically, trajectory planning and control in complex parking scenarios may include the following steps: The first step is to calculate the environmental complexity of the parking start and end points.

[0054] like Figure 2 As shown, the perception information of the parking task is first obtained, including the starting and ending postures of the parking. The posture includes the two-dimensional coordinates of the vehicle and the vehicle heading angle. The vehicle status information and obstacle information are input and parameters are set. Then, the vehicle geometric boundary is expanded to form the vehicle's perception area in each expansion direction, where the expansion directions include forward, backward, left, and right.

[0055] Furthermore, the minimum distance detection is performed in each direction in an iterative manner according to the map resolution, and the minimum distance between the vehicle's extended boundary and the obstacle is recorded. A narrow channel threshold is set. When the minimum distance in a certain direction is less than the narrow channel threshold, the environmental factor in that direction is recorded as 1, otherwise it is recorded as 0.

[0056] Furthermore, weights in the four directions are set according to the vehicle kinematics, and the weight in the forward direction is greater than that in the backward direction, the weight in the backward direction is greater than that in the left direction, and the weight in the left direction is equal to that in the right direction. A nonlinear adjustment parameter with a value between 0.5 and 1 and an amplification factor when obstacles are dense are set. The role of the adjustment parameter and the amplification factor is to weaken the influence of distant obstacles and strengthen the influence of nearby obstacles, so as to enhance the environmental complexity in this direction.

[0057] Furthermore, based on the preset weights and nonlinear adjustment parameters, the environmental complexity contribution values ​​in each direction are calculated, and then weighted sum is performed to obtain the environmental complexity of the current state.

[0058] Specifically, the formula for calculating the environmental complexity contribution value in each expansion direction is as follows: ; in, Indicates the expansion direction, Indicates direction The weight of Indicates an environmental factor with a value of 0 or 1, Indicates direction The minimum distance, represents the adjustment parameter, Represents the amplification factor.

[0059] Through the above process, the embodiment of the present invention can accurately evaluate the environmental complexity within the parking start and end areas in real time, providing a basis for the subsequent front-end search process.

[0060] In the second step, the improved hybrid A* algorithm is used to search for the initial trajectory based on the complexity of the environment.

[0061] Specifically, a simplified 2D bicycle model is first used to describe the vehicle motion and discretized for iterative search. Then, combined with the environmental complexity of the parking starting point and end point, the traditional hybrid A* search algorithm is optimized and improved. The improved hybrid A* search algorithm is divided into forward and reverse search.

[0062] Specifically, when the environmental complexity of the starting point is greater than the environmental complexity of the end point, the forward hybrid A* algorithm is used from the starting point; when the environmental complexity of the starting point is less than the environmental complexity of the end point, the reverse hybrid A* algorithm is used from the end point.

[0063] Among them, the forward search starts from the starting point, expands the vehicle state according to the discrete state update formula, and uses a heuristic function for heuristic search. The cumulative cost in the heuristic function includes kinematic costs such as steering and acceleration changes. The reverse search starts from the end point and also uses a discrete kinematic model to expand the state. At the end of both search algorithms, the Reeds-Shepp curve is used to quickly connect the states, accelerating the search and solution process, and ultimately obtaining a rough initial trajectory with time and space decoupling.

[0064] It is worth noting that the forward search strictly follows the iterative kinematic model when expanding the state, ensuring that each step of the state satisfies the vehicle motion constraints and giving priority to expanding state nodes with lower complexity and higher passability.

[0065] The third step is to construct kinematic constraints based on the discrete kinematic model.

[0066] Specifically, the vehicle kinematics is modeled, the two-dimensional coordinates, speed, steering wheel angle and heading angle of the vehicle are selected as state variables and the state equation is designed, and the state equation is discretized using cubic Hermit.

[0067] The fourth step is to construct a safe driving corridor based on the initial trajectory.

[0068] like Figure 3 As shown in the figure, the entire vehicle is covered by three circles of the same size at the front, middle, and rear. This approach can not only better approximate the actual shape of the vehicle, but also facilitate the subsequent setting of boundary constraints on the center position of the circle.

[0069] Furthermore, the circumscribed circles of the three circles corresponding to each trajectory point are constructed in the reverse direction parallel to the world coordinate system, and are expanded in four directions in sequence to obtain the upper and lower bounds of the safe driving corridor. The coordinates of the three circle centers are constrained with the help of the upper and lower bounds to obtain a simplified completely convex obstacle avoidance constraint.

[0070] Furthermore, the weight of the obstacle avoidance constraint is set to 10 9 , used to emphasize that this constraint should be satisfied first during optimization.

[0071] The fifth step is to calculate the environmental complexity of each discrete trajectory point based on the safe driving corridor, and thus introduce the sampling time classification into the optimization problem.

[0072] Specifically, based on the size of the safe driving corridor and combined with the simplified rigid body characteristics of the vehicle, the environmental complexity of each trajectory point in the initial trajectory is calculated. The trajectory points are divided into two categories: wide and narrow according to the environmental complexity of each trajectory point. These two types of estimated points correspond to two different sampling times, respectively. This sampling time is included in the optimization problem as an optimizable optimization variable to improve the efficiency of solving the optimization problem. At the same time, the impact of environmental complexity on the sampling time is taken into account.

[0073] Specifically, the maximum sampling time of wide points and the minimum sampling time of narrow points are calculated as reference values. The square sum of the errors between the current sampling time and the reference value is calculated and multiplied by the weight as the sampling time constraint to balance the computational efficiency and trajectory quality.

[0074] Step 6: Construct the final optimization problem and solve it to obtain the final spatiotemporal joint parking trajectory.

[0075] Specifically, the kinematic optimization objective, i.e., the kinematic constraint, the sampling time optimization objective, i.e., the sampling time constraint, generated by the environmental complexity, and the trajectory smoothness and comfort optimization objectives are integrated to construct the final constraint conditions of the optimization problem. The interior point method is called to solve the final constraint conditions, and the final spatiotemporal joint planning trajectory containing the state and control quantities corresponding to each trajectory point is obtained. This planning trajectory is the optimal parking trajectory that satisfies both the kinematic constraints and the environmental complexity.

[0076] The seventh step is to perform horizontal and vertical decoupled tracking control on the parking trajectory based on the MPC controller.

[0077] like Figure 4 As shown, first, the current track point to be tracked is prospectively selected from the planned trajectory according to the current state of the vehicle. This track point provides the ideal position and direction information of the vehicle at the current time point, which helps the vehicle to smoothly track the target trajectory.

[0078] Furthermore, the error of the vehicle in the longitudinal direction (usually the direction of vehicle movement) is calculated, and the error calculation is based on the distance between the current vehicle position and the selected tracking trajectory point; the error of the vehicle in the lateral direction (perpendicular to the direction of vehicle movement) is calculated, and the error calculation is based on the lateral distance between the current vehicle position and the selected tracking trajectory point.

[0079] Furthermore, a longitudinal MPC controller is used to calculate a control input based on the longitudinal error to adjust the vehicle's speed and acceleration, thereby reducing the longitudinal error. The longitudinal MPC controller uses the vehicle's state equation for prediction and optimization to generate the optimal control input.

[0080] Furthermore, a lateral MPC controller calculates a control input based on the lateral error to adjust the vehicle's steering angle, thereby reducing the lateral error. The lateral MPC controller also uses the vehicle's state equation for prediction and optimization to generate the optimal control input.

[0081] Furthermore, the ego-vehicle system receives control inputs from the longitudinal and lateral MPC controllers and performs corresponding actions (such as acceleration, deceleration, steering, etc.) to track the reference trajectory.

[0082] Throughout the above process, the MPC algorithm continuously calculates errors, predicts future states, and optimizes control inputs to adjust the vehicle's state in real time so that it can accurately track the reference trajectory. This control strategy helps improve the vehicle's tracking performance and stability in complex environments.

[0083] Through the above process, the embodiment of the present invention achieves stable tracking control of the planned trajectory, improving the success rate and safety of parking. By quantifying environmental complexity, improving the hybrid A* algorithm search, constructing safe driving corridors, constructing and solving optimization problems, and performing MPC tracking control, efficient and safe parking trajectory planning and stable tracking control are achieved, achieving precise control of vehicle parking. This not only improves the parking success rate but also significantly enhances the controllability of the trajectory, allowing the vehicle to complete parking tasks more safely and stably in complex parking scenarios. This effectively solves the problems of low computational efficiency, infeasible trajectory planning results, and poor convergence of the optimization process in complex scenarios encountered by existing technologies.

[0084] The following are device embodiments of the present invention, which can be used to implement the moving body trajectory planning method involved in the present invention. For details not disclosed in the device embodiments of the present invention, please refer to the method embodiments of the moving body trajectory planning method involved in the present invention.

[0085] See also Figure 5 , an embodiment of the present invention provides a moving body trajectory planning device 800.

[0086] The moving object trajectory planning device 800 includes but is not limited to: an environment assessment module 810 , an initial trajectory search module 820 , an obstacle avoidance constraint construction module 830 , a sampling time constraint construction module 840 , a trajectory planning module 850 and a tracking control module 860 .

[0087] The environment assessment module 810 is used to obtain the perception information of the starting point and the end point in the planning task, and perform geometric boundary expansion and minimum distance detection on the moving body based on the perception information to obtain the environmental complexity of the starting point and the end point of the moving body.

[0088] The initial trajectory search module 820 is used to obtain the initial trajectory from the starting point and the end point by a hybrid A* algorithm based on the environment complexity and the bidirectional search strategy, and to perform kinematic modeling on the moving body and construct kinematic constraints.

[0089] The obstacle avoidance constraint construction module 830 is used to construct a safety corridor through a circular approximate moving body based on the initial trajectory, and obtain the obstacle avoidance constraint of the planning task by constraining the coordinates of the center of the circle.

[0090] The sampling time constraint building module 840 is used to calculate the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor, and classify the trajectory points according to the environmental complexity to obtain the sampling time constraint.

[0091] The trajectory planning module 850 is used to obtain the final constraints based on the kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints, and use the initial trajectory as the initial solution to obtain the planned trajectory according to the interior point method.

[0092] The tracking control module 860 is used to decouple the planned trajectory in the lateral and longitudinal directions to obtain the lateral and longitudinal state equations respectively, and use the model predictive control (MPC) algorithm to track the moving body according to the state equations.

[0093] It should be noted that the above embodiments provide trajectory planning for moving bodies, and only illustrate the division of the above functional modules. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the moving body trajectory planning device will be divided into different functional modules to complete all or part of the functions described above.

[0094] In addition, the moving body trajectory planning device and the moving body trajectory planning method provided in the above embodiments belong to the same concept, and the specific manner in which each module performs operations has been described in detail in the method embodiment and will not be repeated here.

[0095] Figure 6 The following is a schematic diagram of the structure of an electronic device according to an exemplary embodiment.

[0096] It should be noted that the electronic device is only an example adapted to the present invention and cannot be considered to provide any limitation on the scope of use of the present invention. The electronic device cannot be interpreted as needing to rely on or must have Figure 6 One or more components of exemplary electronic device 2000 are shown.

[0097] The hardware structure of the electronic device 2000 may vary greatly due to different configurations or performances, such as Figure 6 As shown, the electronic device 2000 includes a power supply 210 , an interface 230 , at least one memory 250 , and at least one central processing unit (CPU) 270 .

[0098] Specifically, the power supply 210 is used to provide operating voltage for various hardware devices on the electronic device 2000 .

[0099] The interface 230 includes at least one wired or wireless network interface 231 for interacting with external devices. Of course, in other examples adapted by the present invention, the interface 230 may further include at least one serial-to-parallel conversion interface 233, at least one input / output interface 235, and at least one USB interface 237, etc. Figure 6 As shown, this does not constitute a specific limitation.

[0100] The memory 250 serves as a carrier for resource storage and can be a read-only memory, random access memory, disk or optical disk, etc. The resources stored thereon include an operating system 251, application 253 and data 255, etc. The storage method can be temporary storage or permanent storage.

[0101] Among them, the operating system 251 is used to manage and control the various hardware devices and application programs 253 on the electronic device 2000, so as to enable the central processing unit 270 to calculate and process the massive data 255 in the memory 250. It can be WindowsServerTM, Mac OS XTM, UnixTM, LinuxTM, FreeBSDTM, etc.

[0102] The application 253 is a computer-readable instruction that performs at least one specific task based on the operating system 251, and may include at least one module ( Figure 6 (not shown), each module may include computer-readable instructions for the electronic device 2000. For example, the moving body trajectory planning device may be considered as an application 253 deployed on the electronic device 2000.

[0103] The data 255 may be signal information, etc., and is stored in the memory 250 .

[0104] The central processing unit 270 may include one or more processors and is configured to communicate with the memory 250 via at least one communication bus to read computer-readable instructions stored in the memory 250, thereby performing operations and processing on the massive amount of data 255 in the memory 250. For example, the method for moving body trajectory planning can be implemented by the central processing unit 270 reading a series of computer-readable instructions stored in the memory 250.

[0105] In addition, the present invention can also be implemented through hardware circuits or hardware circuits combined with software. Therefore, the implementation of the present invention is not limited to any specific hardware circuits, software, or combination thereof.

[0106] See also Figure 7 In an embodiment of the present invention, an electronic device 4000 is provided. The electronic device 4000 may include: a desktop computer, a laptop computer, a server, etc. with sensor recognition capabilities.

[0107] exist Figure 7 In the embodiment, the electronic device 4000 includes at least one processor 4001 and at least one memory 4003.

[0108] The data exchange between the processor 4001 and the memory 4003 can be achieved through at least one communication bus 4002. The communication bus 4002 may include a path for transmitting data between the processor 4001 and the memory 4003. The communication bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus. The communication bus 4002 can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 Only one thick line is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0109] Optionally, the electronic device 4000 may further include a transceiver 4004, which may be used for data exchange between the electronic device and other electronic devices, such as data transmission and / or data reception. It should be noted that in actual applications, the number of transceivers 4004 is not limited to one, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of the present invention.

[0110] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or other programmable logic device, transistor logic device, hardware component, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with the present disclosure. Processor 4001 may also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, and the like.

[0111] The memory 4003 can be a ROM (Read Only Memory) or other types of static storage devices that can store static information and instructions, a RAM (Random Access Memory) or other types of dynamic storage devices that can store information and instructions, or an EEPROM (Electrically Erasable Programmable Read Only Memory), a CD-ROM (Compact Disc Read Only Memory) or other optical disk storage, optical disc storage (including compact disc, laser disc, optical disc, digital versatile disc, Blu-ray disc, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store desired program instructions or codes in the form of instructions or data structures and can be accessed by the electronic device 4000, but is not limited to these.

[0112] Computer-readable instructions are stored in the memory 4003 , and the processor 4001 can read the computer-readable instructions stored in the memory 4003 through the communication bus 4002 .

[0113] The computer-readable instructions are executed by one or more processors 4001 to implement the moving body trajectory planning method in the above embodiments.

[0114] In addition, an embodiment of the present invention provides a storage medium having computer-readable instructions stored thereon. The computer-readable instructions are executed by one or more processors to implement the moving body trajectory planning method as described above.

[0115] In an embodiment of the present invention, a computer program product is provided. The computer program product includes computer-readable instructions, which are stored in a storage medium. One or more processors of an electronic device read the computer-readable instructions from the storage medium, load and execute the computer-readable instructions, so that the electronic device implements the moving body trajectory planning method described above.

[0116] Compared with the related art, the present invention has the following beneficial effects: 1. The present invention first obtains the perception information of the starting point and the end point in the planning task, performs geometric boundary expansion and minimum distance detection on the moving body, thereby accurately evaluating the complexity of the environment. On this basis, a hybrid A* algorithm based on environmental complexity and bidirectional search strategy is used to obtain the initial trajectory from the starting point and the end point, and kinematic modeling is performed on the moving body to construct kinematic constraints. Then, based on the initial trajectory, a safe corridor is constructed by circularly approximating the moving body to obtain obstacle avoidance constraints. At the same time, the environmental complexity of each trajectory point in the initial trajectory is calculated according to the safe corridor, and the trajectory points are classified by sampling time to obtain sampling time constraints. Finally, the comprehensive kinematic constraints are The proposed method combines the constraints of anti-collision force, obstacle avoidance, sampling time, trajectory smoothness and comfort optimization, uses the interior point method to obtain the planned trajectory, and decouples the planned trajectory in the lateral and longitudinal directions to obtain the lateral and longitudinal state equations respectively. The model predictive control (MPC) algorithm is used to track and control the moving body, thus achieving precise control of the moving body. This not only improves the success rate of the moving body control, but also significantly enhances the controllability of the trajectory, so that the moving body can complete the control task more safely and stably in complex scenarios, thereby effectively solving the problems of low computational efficiency, infeasible trajectory planning results and poor convergence of the optimization process in the existing technology when facing complex scenarios.

[0117] 2. By proactively selecting tracking trajectory points and calculating longitudinal and lateral errors in real time, the MPC algorithm can precisely adjust the vehicle's speed and steering, thereby improving the vehicle's tracking accuracy relative to the reference trajectory. The MPC algorithm uses the vehicle's state equation for prediction and optimization, taking into account future state changes in advance and generating optimal control inputs, thereby enhancing the stability and robustness of the system.

[0118] 3. The present invention uses the MPC algorithm to process information about the vehicle's surrounding environment in real time and dynamically adjust the control strategy based on environmental changes, enabling the vehicle to maintain good tracking performance in complex environments. Through prediction and optimization, the MPC algorithm can generate optimal control inputs, reduce unnecessary control actions, lower energy consumption, and improve the vehicle's energy efficiency.

[0119] 4. The present invention can help a moving object stay on the correct driving trajectory through precise tracking control and real-time error calculation, reducing the risk of deviation from the trajectory, thereby improving safety. It can be customized and adjusted according to different vehicle models and mission requirements, has strong flexibility and scalability, and is applicable to a variety of scenarios.

[0120] It should be understood that although the steps in the flowcharts of the accompanying drawings are shown in sequence as indicated by the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the flowcharts of the accompanying drawings may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

[0121] The above descriptions are only partial embodiments of the present invention. It should be pointed out that ordinary technicians in this technical field can make several improvements and modifications without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. A moving body trajectory planning method, characterized in that: The method comprises: Acquire sensory information of the starting point and the end point of the planning task, and perform geometric boundary expansion and minimum distance detection on the moving body based on the sensory information to obtain the environmental complexity of the starting point and the end point of the moving body; An initial trajectory is obtained by using a hybrid A* algorithm based on the complexity of the environment and a bidirectional search strategy, and kinematic modeling is performed on the moving body to construct kinematic constraints; Based on the initial trajectory, a safety corridor is constructed by approximating the moving body by a circle, and an obstacle avoidance constraint of the planning task is obtained by constraining the coordinates of the center of the circle; Calculating the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor, and classifying the trajectory points by sampling time according to the environmental complexity to obtain sampling time constraints; Obtaining final constraints according to the kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints, and obtaining a planned trajectory according to an interior point method using the initial trajectory as an initial solution; The planned trajectory is decoupled in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, and a model predictive control (MPC) algorithm is used to track and control the moving body according to the state equations.

2. The moving body trajectory planning method according to claim 1, wherein: The acquiring of the perception information of the starting point and the end point in the planning task, and performing geometric boundary expansion and minimum distance detection on the moving body according to the perception information to obtain the environmental complexity of the starting point and the end point of the moving body, includes: Acquiring sensory information for a planned task, wherein the planned task includes two-dimensional coordinates and heading angles of the moving body at a starting point and an end point; Expanding the geometric model of the moving object along an expansion direction according to the actual size of the moving object and the map resolution to obtain the perception areas of the moving object at the starting point and the end point; the expansion directions include forward, backward, left, and right directions; Obtaining the minimum distance between the moving object and the obstacle in the expansion direction within the perception area in an iterative manner according to the map resolution, and setting a distance threshold; The weight and environmental factor of the moving body in the expansion direction are respectively set according to the kinematics of the moving body and the distance threshold, and a nonlinear adjustment parameter and an amplification factor are set; the amplification factor is used to enhance the environmental complexity of the corresponding expansion direction when obstacles are dense; The environmental complexity contribution value of the moving body in the extension direction is calculated according to the weight, environmental factor, minimum distance, adjustment parameter and amplification factor, and the environmental complexity of the starting point and end point of the moving body is obtained by weighted summation of the environmental complexity contribution values.

3. The moving body trajectory planning method according to claim 2, wherein: The formula for calculating the environmental complexity contribution value in the expansion direction includes: ； in, Indicates the expansion direction, Indicates direction The weight of Indicates an environmental factor with a value of 0 or 1, Indicates direction The minimum distance represents the adjustment parameter, Represents the amplification factor.

4. The moving body trajectory planning method according to claim 1, wherein: The hybrid A* algorithm based on the complexity of the environment and the bidirectional search strategy obtains an initial trajectory, and performs kinematic modeling on the moving body to construct kinematic constraints, including: When the environmental complexity of the starting point is greater than the environmental complexity of the end point, the forward hybrid A* algorithm is used from the starting point; when the environmental complexity of the starting point is less than the environmental complexity of the end point, the reverse hybrid A* algorithm is used from the end point to obtain the initial trajectory; Kinematic modeling is performed on the moving body to determine state variables, a continuous state equation of the moving body is constructed based on the state variables, the state equation is discretized using a cubic Hermit algorithm, and then the initial trajectory is optimized in a spatiotemporal and temporal manner using the state equation; the state variables include the two-dimensional coordinates, velocity, steering wheel angle, and heading angle of the moving body.

5. The moving body trajectory planning method according to claim 1, wherein: The step of constructing a safe corridor by approximating the moving body through a circle based on the initial trajectory, and obtaining an obstacle avoidance constraint for the planning task by constraining the coordinates of the center of the circle, includes: Using three circles of the same size to cover the moving body, for each trajectory point in the initial trajectory, the three circles corresponding to the outer circle are iteratively constructed in a direction parallel to the world coordinate system and sequentially expanded to obtain the boundary of the safety corridor; According to the boundary of the safety corridor, the coordinates of the center of the circle are set with a boundary constraint to obtain the obstacle avoidance constraint of the planning task, and the weight of the obstacle avoidance constraint is set to 10. 9 .

6. The moving body trajectory planning method according to claim 1, wherein: The step of calculating the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor, and classifying the trajectory points by sampling time according to the environmental complexity to obtain sampling time constraints, includes: Calculating the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor; the environmental complexity reflects the narrowness of the space around the trajectory point of the moving object or the density of obstacles; The trajectory points are divided into wide points and narrow points according to the complexity of the environment, the maximum sampling time of the wide points and the minimum sampling time of the narrow points are calculated as reference values, and the square sum of the errors between the current sampling time and the reference value is calculated and multiplied by the weight as the sampling time constraint.

7. The moving body trajectory planning method according to claim 1, wherein: Decoupling the planned trajectory in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, and using a model predictive control algorithm (MPC) to track and control the moving body according to the state equations, including: Decoupling the planned trajectory in the lateral and longitudinal directions to obtain lateral and longitudinal state equations, respectively; selecting a tracking trajectory point from the planned trajectory according to the current state of the moving body; and performing lateral error calculation and longitudinal error calculation on the tracking trajectory point according to the state equations to obtain a lateral error and a longitudinal error; The moving body is laterally controlled according to the lateral error by a lateral MPC controller, and the moving body is longitudinally controlled according to the longitudinal error by a longitudinal MPC controller, and the lateral error and the longitudinal error are fed back according to the operating state of the moving body and the state equation.

8. A moving body trajectory planning device, characterized in that: The device comprises: An environmental assessment module is used to obtain sensory information of the starting point and the end point of the planning task, and to perform geometric boundary expansion and minimum distance detection on the moving body based on the sensory information to obtain the environmental complexity of the starting point and the end point of the moving body; An initial trajectory search module is used to obtain an initial trajectory based on a hybrid A* algorithm of the environment complexity and a bidirectional search strategy, and to perform kinematic modeling on the moving body to construct kinematic constraints; An obstacle avoidance constraint construction module, configured to construct a safe corridor by approximating the moving body through a circle based on the initial trajectory, and obtain the obstacle avoidance constraint of the planning task by constraining the coordinates of the center of the circle; a sampling time constraint construction module, configured to calculate the environmental complexity of each trajectory point in the initial trajectory according to the safety corridor, and classify the trajectory points according to the environmental complexity to obtain a sampling time constraint; A trajectory planning module is used to obtain the final constraints based on the kinematic constraints, obstacle avoidance constraints, sampling time constraints, trajectory smoothness and comfort optimization constraints, and use the initial trajectory as an initial solution to obtain the planned trajectory according to the interior point method; The tracking control module is used to decouple the planned trajectory in the lateral and longitudinal directions to obtain lateral and longitudinal state equations respectively, and use the model predictive control (MPC) algorithm to track and control the moving body according to the state equations.

9. An electronic device, characterized in that: include: at least one processor and at least one memory, wherein: The memory has computer-readable instructions stored thereon; The computer-readable instructions are executed by one or more processors, so that the electronic device implements the moving body trajectory planning method according to any one of claims 1 to 7.

10. A storage medium having computer-readable instructions stored thereon, characterized in that: The computer-readable instructions are executed by one or more processors to implement the moving body trajectory planning method according to any one of claims 1 to 7.

Images