A motion optimization obstacle avoidance control method and system for a car-like robot
By combining the improved hybrid A* algorithm and Hermite curve fitting with duality theory, the computational complexity and trajectory smoothness problems of obstacle avoidance planning for vehicle-like robots in complex environments are solved, achieving efficient and robust obstacle avoidance control.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- SHANDONG UNIV
- Filing Date
- 2026-04-02
- Publication Date
- 2026-06-02
AI Technical Summary
Existing technologies struggle to generate smooth, continuous, and collision-free trajectories while satisfying the dynamics and actuator constraints of vehicle-like robots. In particular, they struggle to achieve efficient and robust motion planning and obstacle avoidance control in complex environments. Traditional methods are computationally intensive or unsuitable for large-scale robot systems.
An improved hybrid A* algorithm is used to plan a global path in three-dimensional space, and Hermite curves are used to fit sparse state sequences. By combining duality theory, the geometric separation condition is transformed into a differentiable nonlinear algebraic constraint, and a trajectory optimization objective function is constructed to satisfy dynamic, input, and acceleration smoothness constraints, thereby realizing obstacle avoidance planning and control.
It enables real-time obstacle avoidance control of polygonal obstacles in complex environments, reduces computational load, improves planning efficiency and trajectory smoothness, and is suitable for large-scale robot systems.
Smart Images
Figure CN122131771A_ABST
Abstract
Description
Technical Field
[0001] This disclosure relates to the field of robot path planning and obstacle avoidance technology, specifically to a motion optimization obstacle avoidance control method and system for vehicle-like robots. Background Technology
[0002] The statements in this section are merely background information relating to this disclosure and do not necessarily constitute prior art.
[0003] With the rapid development of intelligent mobile systems such as autonomous vehicles, warehousing and logistics robots, and service robots, achieving safe and efficient motion planning and obstacle avoidance control in complex and dynamic environments has become a core technical challenge in this field. For robot-like systems, their motion is constrained by the Ackerman steering model, exhibiting nonholonomic constraints such as minimum turning radius and inability to move laterally, making traditional grid-based path planning methods difficult to apply directly. Furthermore, real-world environments often contain obstacles with complex shapes, requiring robots to generate a smooth, continuous, and collision-free trajectory while satisfying dynamic and actuator constraints. This places high demands on the real-time performance, feasibility, and robustness of planning algorithms.
[0004] In existing technologies, hybrid A* algorithms can generate feasible paths that meet nonholonomic constraints by expanding nodes by introducing a vehicle kinematics model into a continuous space. However, traditional hybrid A* algorithms typically use small expansion step sizes to ensure search accuracy, which leads to a rapid expansion of the search tree size, a significant increase in computational cost, and makes it difficult to scale to large-scale robotic systems.
[0005] On the other hand, in terms of local obstacle avoidance and trajectory tracking, Model Predictive Control (MPC) has been widely studied because it can handle both state and control constraints simultaneously. However, when obstacles and robot shapes are modeled as polygons or polyhedra, traditional approaches often use penalty functions or heuristic safety distance methods for approximation. These methods not only struggle to guarantee strict collision-free behavior, but also tend to cause severe shrinkage of the feasible region when obstacles are dense or the robot is large, leading to solution failures or control oscillations.
[0006] In recent years, existing technologies have attempted to transform the geometric distance problem into a differentiable algebraic constraint using duality theory, thereby achieving optimization-based collision avoidance control. However, the following limitations still exist: (1) Most of the relevant methods are only applicable to point robots or circular models, and are difficult to directly extend to vehicle-like robots with arbitrary convex polygon shapes.
[0007] (2) At the same time, existing work usually focuses on the improvement of the control level and pays insufficient attention to the computational complexity of the global path planning stage. There is still a lack of a systematic solution that can take into account planning efficiency, trajectory smoothness and unified modeling of obstacle avoidance constraints of full-size robots. Summary of the Invention
[0008] To address the aforementioned issues, this disclosure proposes a motion optimization obstacle avoidance control method and system for vehicle-like robots. This method can achieve obstacle avoidance planning and control of polygonal obstacles while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot.
[0009] According to some embodiments, the present disclosure adopts the following technical solutions: A motion optimization obstacle avoidance control method for vehicle-like robots includes: Define the state space of the vehicle-type robot; An improved hybrid A* algorithm is used to perform heuristic search in continuous three-dimensional space to plan the global path of the vehicle-like robot; Based on the global path, Hermite curves are used to fit the sparse state sequence to obtain a time-space continuous trajectory. Model the space occupied by the vehicle-like robot and obstacles, and establish a collision model between the vehicle-like robot and obstacles; Based on the collision model, the geometric separation conditions between the robot and the obstacle are transformed into a set of continuously differentiable nonlinear algebraic constraints according to duality theory. The time-space continuous trajectory is used as the reference trajectory to construct the trajectory optimization objective function. This function can solve the motion control variables of the vehicle-like robot while satisfying the dynamic constraints, input constraints and acceleration smoothing constraints of the vehicle-like robot, thus realizing the obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
[0010] According to some embodiments, the present disclosure adopts the following technical solutions: A motion optimization obstacle avoidance control system for vehicle-like robots includes: The initialization module is used to define the state space of the vehicle-like robot; The global path planning module is used to plan the global path of the vehicle-like robot by performing a heuristic search in a continuous three-dimensional space using an improved hybrid A* algorithm. The trajectory fitting module is used to fit sparse state sequences based on global paths using Hermite curves to obtain time-space continuous trajectories. The collision model building module is used to model the space occupied by the vehicle-like robot and obstacles, and to establish a collision model between the vehicle-like robot and obstacles. An optimized obstacle avoidance control module is used to transform the geometric separation conditions between the robot and obstacles into a set of continuously differentiable nonlinear algebraic constraints based on a collision model and duality theory. Using the time-space continuous trajectory as a reference trajectory, a trajectory optimization objective function is constructed. This module can solve the motion control variables of the vehicle-like robot while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot, thus realizing obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
[0011] According to some embodiments, the present disclosure adopts the following technical solutions: A computer program product includes a computer program that, when executed by a processor, implements the aforementioned motion optimization obstacle avoidance control method for vehicle-like robots.
[0012] According to some embodiments, the present disclosure adopts the following technical solutions: A non-transitory computer-readable storage medium is provided for storing computer instructions, which, when executed by a processor, implement the aforementioned motion optimization obstacle avoidance control method for a vehicle-like robot.
[0013] According to some embodiments, the present disclosure adopts the following technical solutions: An electronic device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to implement the motion optimization obstacle avoidance control method for a vehicle-like robot.
[0014] Compared with the prior art, the beneficial effects of this disclosure are as follows: This disclosure presents a motion optimization obstacle avoidance control method for vehicle-like robots. It uses an improved hybrid A* algorithm to generate an initial path that conforms to the Ackerman kinematic model and uses Hermite curve fitting to fit the path. This method not only strictly follows the heading angle constraint between two points, but also makes the robot's speed change more gradual during the start-up and stopping phases by adjusting the magnitude of the direction vector. This effectively suppresses sudden speed changes at the trajectory endpoints and reduces the burden on the actuators during start-up and stopping.
[0015] This disclosure presents a motion optimization obstacle avoidance control method for vehicle-like robots. The method models the obstacles and the robot's shape as convex polygons. Based on duality theory, the geometric separation conditions between the robot and the obstacles are transformed into a set of continuously differentiable nonlinear algebraic constraints. This allows the obstacle avoidance constraints to be solved efficiently in nonlinear optimization problems (such as NMPC), thereby realizing real-time planning and obstacle avoidance control of full-size robots.
[0016] This disclosure discloses a motion optimization obstacle avoidance control method for vehicle-like robots. While satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot, it uses a solver to solve an optimization problem that includes obstacle constraints, kinematic constraints, and control input constraints to obtain the control input. The robot executes the first control variable and performs rolling optimization. In this way, it is not necessary to explicitly calculate the minimum distance. Instead, a feasible control input is automatically found through the optimization problem, guiding the robot to follow the reference trajectory while avoiding obstacles along the way, thus realizing obstacle avoidance planning and control for polygonal obstacles. Attached Figure Description
[0017] The accompanying drawings, which form part of this disclosure, are used to provide a further understanding of this disclosure. The illustrative embodiments of this disclosure and their descriptions are used to explain this disclosure and do not constitute an undue limitation of this disclosure.
[0018] Figure 1 This is a schematic diagram of Hermite curve fitting according to an embodiment of the present disclosure; Figure 2 This is a reference trajectory generated in the embodiments of this disclosure; Figure 3 This is a simulation diagram of a method according to an embodiment of the present disclosure; Figure 4 is a diagram illustrating a practical demonstration of the method according to an embodiment of this disclosure. In Figure 4, (a) is the entity model diagram; and (b) is the demonstration trajectory diagram. Detailed Implementation
[0019] The present disclosure will be further described below with reference to the accompanying drawings and embodiments.
[0020] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of this disclosure. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains.
[0021] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this disclosure. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms “comprising” and / or “including” are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.
[0022] Example 1 One embodiment of this disclosure provides a motion optimization obstacle avoidance control method for vehicle-like robots, the method steps of which include: Step 1: Define the state space of the vehicle-like robot; Step 2: Using an improved hybrid A* algorithm, a heuristic search is performed in continuous three-dimensional space to plan the global path of the vehicle-like robot; Step 3: Based on the global path, use Hermite curves to fit the sparse state sequence to obtain a time-space continuous trajectory; Step 4: Model the space occupied by the car-like robot and obstacles, and establish a collision model between the car-like robot and obstacles; Step 5: Based on the collision model, the geometric separation conditions between the robot and the obstacle are transformed into a set of continuously differentiable nonlinear algebraic constraints according to duality theory. The time-space continuous trajectory is used as the reference trajectory to construct the trajectory optimization objective function. This function can solve the motion control quantities of the vehicle-like robot while satisfying the dynamic constraints, input constraints and acceleration smoothing constraints of the vehicle-like robot, thus realizing the obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
[0023] As one embodiment, the disclosed motion optimization obstacle avoidance control method for vehicle-like robots can achieve obstacle avoidance planning and control for polygonal obstacles while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot. The specific implementation process is as follows: Step 1: Define the state space of the vehicle-like robot; Specifically, let x , y These are the coordinates of the robot's rear axis center in the world coordinate system. θ Add a discrete time dimension to the robot's heading angle for each robot. t That is, the robot's state space is [ x , y , θ , t Each time a node expands, the time of the child node increases by a fixed Δ. t .
[0024] Step 2: Using an improved hybrid A* algorithm, a heuristic search is performed in continuous three-dimensional space to plan the global path of the vehicle-like robot; Specifically, (1) an improved hybrid A* algorithm is used in a continuous three-dimensional space [ x , y , θ A heuristic search is performed, with each expansion's child nodes derived from six motion primitives: forward, backward, forward left turn, backward left turn, forward right turn, and backward right turn, maintaining a fixed front wheel steering angle during turns. Each expansion moves a fixed distance. s To improve solution efficiency and facilitate the scaling of the algorithm to large-scale clusters, this disclosure... sIt needs to be longer than the traditional hybrid A* algorithm, meaning the path points are sparser. In practice, it is generally taken as 1-2 times the robot radius.
[0025] The improved hybrid A* algorithm of this disclosure incorporates a time allocation mechanism, assigning corresponding time information to each trajectory point, whereas traditional hybrid A* lacks this mechanism. Furthermore, the improved hybrid A* algorithm uses a larger step size during node expansion and subsequently employs Hermite curve fitting, saving computation time. In contrast, traditional hybrid A* generates dense trajectory points with smaller step sizes and lacks a fitting step. The improved hybrid A* algorithm of this disclosure terminates the trajectory as a Reed-Shepp curve, and a strategy of uniformly dividing the trajectory into unidirectional segments is proposed to evenly distribute time.
[0026] Furthermore, the priority of node expansion is determined by the cost value for each node. n Its cost is:
[0027] in, g ( n The cost is the actual cumulative cost from the starting point to this node, specifically the cost from the parent node. g ( n The value plus the movement cost, for a forward movement, is the fixed distance that will be moved. s As a consequence, turning and reversing actions must be multiplied by the corresponding penalty coefficients. h ( n ) is a heuristic function:
[0028] in, h RS ( n ) represents the distance to the target point considering only the robot's nonholonomic constraints, without considering obstacle constraints; specifically, it is the length of the Reed-Shepp curve connecting the current node to the target point. Conversely, h obs ( n Ignoring nonholonomic constraints and only considering obstacle constraints, the shortest distance to the target point is calculated using Dijkstra's algorithm on a two-dimensional grid map.
[0029] If the current node is close enough to the target point, it is directly connected to the target point using a Reed-Shepp curve. Reed-Shepp is a shortest path planning model for nonholonomic robots, describing the shortest path between two points under the constraint of minimum turning radius. Essentially, Reed-Shepp is a finite combination of straight lines and circular arcs with fixed curvature, requiring at most 5 segments to connect any two poses.
[0030] (2) Because each waypoint carries time information. t Therefore, the Reed-Shepp curve is discretized to adapt to time allocation requirements. The original curve is divided into segments in the same direction (i.e., continuous forward or continuous backward segments). For each segment in the same direction, an equal-length segmentation strategy is adopted to ensure that there are similar trajectory lengths within the same time interval, thereby ensuring the uniformity of speed. The number of segments is:
[0031] in L same-dir The length of the segment in the same direction. This is a rounding operator. A refined discrete state sequence is obtained by recursively calculating the pose at the end of each segment.
[0032] Step 3: Based on the global path, use Hermite curves to fit the sparse state sequence to obtain a time-space continuous trajectory; The sparse state sequence of the global path cannot be directly followed by the robot. Hermite curves are used for fitting to obtain a time-space continuous trajectory. The shape of the Hermite curve is determined by the start point, end point, and their tangent vector. Its mathematical expression is:
[0033] in, p 0, p 1 represents the coordinates of two adjacent points in the trajectory; m 0, m 1 represents the direction vector at the two points; t 0, t 1 represents the time at the two points. Compared to Bézier curves and B-spline curves commonly used in robot trajectory planning, Hermite curves, while satisfying the requirement of being differentiable everywhere, strictly follow the position and direction vectors at the start and end points. The heading angle at any position in the trajectory can be obtained by differentiating the trajectory.
[0034] Direction vector m 0, m 1 specifies the first derivative of the start and end points of the sub-segment. Its magnitude represents the velocity and determines the motion trend of the curve at the endpoints. To ensure smoother velocity changes during the robot's start and stop phases, a direction vector with a smaller magnitude is assigned to the start and end points of each segment in the same direction during fitting, making the robot's velocity changes more gradual.
[0035] in, Figure 1The figure shows the fitting results of the Hermite curve. The arrows in the figure represent the generated discrete global path points. Green arrows represent the start and end poses of the same-direction segment, while blue arrows represent the poses within the same-direction segment. The trajectory contains two same-direction segments with equal time intervals between adjacent trajectory points. Due to the smaller modulus at the beginning and end of the same-direction segments, the velocity at these points is relatively low, resulting in a smaller spacing between trajectory points. This strategy can effectively suppress sudden velocity changes at the trajectory endpoints, reducing the burden on the actuator during startup and shutdown.
[0036] Step 4: Model the space occupied by the car-like robot and obstacles, and establish a collision model between the car-like robot and obstacles; Specifically, (1) the space occupied by the robot and obstacles is modeled as a convex polygon:
[0037] In two-dimensional space, assume the obstacle is made of l The edges are formed, in the formula o This represents the coordinates of any point in the space occupied by the obstacle in the global coordinate system. The matrix representing the normal vectors of each side of the obstacle; This represents the half-space bias vector. For example, the space occupied by a rectangular obstacle is:
[0038] but A , b They are respectively:
[0039] Similarly, a robot is defined in its own coordinate system as:
[0040] Let the coordinates of the robot's center point in the global coordinate system be... Transform the robot's spatial coordinates to the global coordinate system:
[0041] in:
[0042] At this point, the robot and the obstacles are described as polyhedra with piecewise linear boundaries that are not differentiable.
[0043] Step 5: Based on the collision model, and according to duality theory, transform the geometric separation conditions between the robot and obstacles into a set of continuously differentiable nonlinear algebraic constraints; specific steps include: (1) First, define the minimum distance between the robot and the obstacle:
[0044] make ,but:
[0045] in, z This represents any point in the space occupied by the robot in its own coordinate system; e = Rz + t This indicates that the space occupied by the robot is being transformed to the global coordinate system. Therefore, e Let be any point in the space occupied by the robot in the global coordinate system; o An obstacle occupies any point in space within the global coordinate system. dist ( E , O ) represents the distance between the two closest points between the robot and the obstacle. The subsequent optimization problem is also constructed by making this distance greater than 0, so as to achieve obstacle avoidance.
[0046] (2) The distance function described above is neither differentiable nor derivative in the MPC (Model Predictive Control) controller, and therefore cannot be directly used as a constraint. To transform it into a form that is differentiable without min and contains only algebraic constraints, a Lagrangian function is introduced based on duality theory:
[0047] Among them, the newly introduced variables These are Lagrange multipliers. At the optimal point, the gradient must be zero.
[0048]
[0049] remember In a geometric sense, n This represents the unit normal vector pointing from the obstacle to the robot. Therefore, we obtain:
[0050] At the same time, we can obtain:
[0051] Therefore, the distance between the robot and the obstacle can be obtained as:
[0052] so:
[0053] Similarly:
[0054] Will and Substitution get:
[0055] Thus, a non-smooth minimum distance constraint is transformed into a continuously differentiable algebraic constraint system:
[0056] in, This is to avoid dual variables during the solution process. Unbounded magnification leads to a false increase in distance.
[0057] Step 6: Using the time-space continuous trajectory as a reference trajectory, construct a trajectory optimization objective function. This function can solve for the motion control quantities of the vehicle-like robot while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot, thus realizing obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
[0058] (1) Using the trajectory obtained by fitting at the planning level as the reference trajectory, predict the motion trajectory from time 0 to N-1, and obtain the objective function of the optimization problem:
[0059] in, They represent k The robot's actual pose and reference pose at any given moment; express k For vehicle-type robots, the control input at any given moment includes linear velocity. v and front wheel cornering ; express k Constantly control the differential of the input. Q, R Let be the weight matrix. This cost function minimizes the tracking error of the actual trajectory relative to the reference trajectory, the magnitude of the control input, and the rate of change of the control.
[0060] (2) Considering the kinematic model of the vehicle-like robot, the following state constraints are added:
[0061] in, L The wheelbase is the distance between the front and rear axes of the robot, Δ. t This represents the time interval between the two states.
[0062] (3) Considering the robot's own physical limitations and motion smoothness, the following control input constraints are added:
[0063] (4) Finally, solve the optimization problem containing obstacle constraints, kinematic constraints, and control input constraints using solvers such as IPOPT to obtain the control input. The robot executes the first control input and then performs a rolling optimization. In this way, it is not necessary to explicitly calculate the minimum distance. Instead, a feasible control input is automatically found through the optimization problem, guiding the robot to follow the reference trajectory while avoiding obstacles along the way.
[0064] in, Figure 3 The simulation diagram of this disclosure is shown, in which the gray dashed line is the reference trajectory generated by the method, the black rectangle is the newly added obstacle used to test the obstacle avoidance performance of the algorithm, and the blue rectangle records the historical pose of the robot. Figure 4 is a real-machine demonstration diagram of the method, in which the dashed line in (b) is the reference trajectory and the solid line is the actual trajectory.
[0065] Example 2 One embodiment of this disclosure provides a motion optimization obstacle avoidance control system for vehicle-like robots, comprising: The initialization module is used to define the state space of the vehicle-like robot; The global path planning module is used to plan the global path of the vehicle-like robot by performing a heuristic search in a continuous three-dimensional space using an improved hybrid A* algorithm. The trajectory fitting module is used to fit sparse state sequences based on global paths using Hermite curves to obtain time-space continuous trajectories. The collision model building module is used to model the space occupied by the vehicle-like robot and obstacles, and to establish a collision model between the vehicle-like robot and obstacles. An optimized obstacle avoidance control module is used to transform the geometric separation conditions between the robot and obstacles into a set of continuously differentiable nonlinear algebraic constraints based on a collision model and duality theory. Using the time-space continuous trajectory as a reference trajectory, a trajectory optimization objective function is constructed. This module can solve the motion control variables of the vehicle-like robot while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot, thus realizing obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
[0066] Example 3 One embodiment of this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the aforementioned motion optimization obstacle avoidance control method for vehicle-like robots.
[0067] Example 4 One embodiment of this disclosure provides a non-transitory computer-readable storage medium for storing computer instructions. When these computer instructions are executed by a processor, they implement the motion optimization obstacle avoidance control method for a vehicle-like robot.
[0068] Example 5 One embodiment of this disclosure provides an electronic device, including a processor, a memory, and a computer program; wherein the processor is connected to the memory, and the computer program is stored in the memory. When the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to implement the motion optimization obstacle avoidance control method for a vehicle-like robot.
[0069] This disclosure is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this disclosure. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0070] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.
[0071] While the specific embodiments of this disclosure have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of this disclosure. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of this disclosure are still within the scope of protection of this disclosure.
Claims
1. A motion optimization obstacle avoidance control method for vehicle-like robots, characterized in that, include: Define the state space of the vehicle-type robot; An improved hybrid A* algorithm is used to perform heuristic search in continuous three-dimensional space to plan the global path of the vehicle-like robot; Based on the global path, Hermite curves are used to fit the sparse state sequence to obtain a time-space continuous trajectory. Model the space occupied by the vehicle-like robot and obstacles, and establish a collision model between the vehicle-like robot and obstacles; Based on the collision model, the geometric separation conditions between the robot and the obstacle are transformed into a set of continuously differentiable nonlinear algebraic constraints according to duality theory. The time-space continuous trajectory is used as the reference trajectory to construct the trajectory optimization objective function. This function can solve the motion control variables of the vehicle-like robot while satisfying the dynamic constraints, input constraints and acceleration smoothing constraints of the vehicle-like robot, thus realizing the obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
2. The motion optimization obstacle avoidance control method for vehicle-like robots as described in claim 1, characterized in that, The defined state space of the vehicle-type robot includes: make x , y These are the coordinates of the robot's rear axis center in the world coordinate system. θ Add a discrete time dimension to the robot's heading angle for each robot. t That is, the robot's state space is [ x , y , θ , t Each time a node expands, the time of the child node increases by a fixed Δ. t .
3. The motion optimization obstacle avoidance control method for vehicle-like robots as described in claim 1, characterized in that, The improved hybrid A* algorithm is used to perform heuristic search in continuous three-dimensional space to plan the global path of the vehicle-like robot, including: An improved hybrid A* algorithm is employed in a continuous three-dimensional space. x , y , θ The heuristic search is performed in the [ ], and the child nodes of each expansion originate from 6 motion primitives, including forward, backward, forward left turn, backward left turn, forward right turn, and backward right turn; a fixed front wheel steering angle is maintained when turning; and a fixed distance is moved during each expansion. s ; The priority of node expansion is determined by the cost value. If the current node is close enough to the target point, it is directly connected to the target point using a Reed-Shepp curve. The Reed-Shepp curve is discretized to adapt to the time allocation requirements, and an equal-length segmentation strategy is adopted to ensure that the trajectory lengths are similar within the same time interval.
4. The motion optimization obstacle avoidance control method for vehicle-like robots as described in claim 1, characterized in that, Based on the global path, Hermite curves are used to fit the sparse state sequence to obtain a time-space continuous trajectory, including: The shape of the Hermite curve is determined by its starting point, ending point, and tangent vector. The Hermite curve satisfies the requirement of being differentiable everywhere, while strictly following the position and direction vectors at the start and end points. The heading angle at any position in the trajectory can be obtained by differentiating the trajectory. The direction vector specifies the first derivative of the start and end points of the sub-segment, and its magnitude represents the speed, determining the motion trend of the curve at the endpoints. To ensure that the robot's speed changes more smoothly during the start and stop phases, a direction vector with a smaller magnitude is assigned to the start and end points of each segment in the same direction during fitting, making the robot's speed change more smooth.
5. The motion optimization obstacle avoidance control method for vehicle-like robots as described in claim 1, characterized in that, Model the space occupied by the vehicle-like robot and obstacles, and establish a collision model between the vehicle-like robot and obstacles, including: The space occupied by the robot and obstacles is modeled as a convex polygon; in two-dimensional space, it is assumed that the obstacles are formed by... l The edge is defined in the global coordinate system as the coordinate value of any point in the space occupied by the obstacle; Transform the spatial coordinates occupied by the robot to the global coordinate system. At this point, the robot and the obstacles are described as polyhedra with piecewise linear boundaries that are not differentiable.
6. The motion optimization obstacle avoidance control method for vehicle-like robots as described in claim 1, characterized in that, Based on the collision model, and according to duality theory, the geometric separation conditions between the robot and obstacles are transformed into a set of continuously differentiable nonlinear algebraic constraints, including: Define the minimum distance between the robot and the obstacle. The distance function is not differentiable or differentiable in the MPC controller and cannot be directly used as a constraint. In order to transform it into a form that does not contain min, is differentiable, and contains only algebraic constraints, we introduce the Lagrangian function according to duality theory, and transform a non-smooth minimum distance constraint into a continuously differentiable algebraic constraint system.
7. A motion optimization obstacle avoidance control system for vehicle-like robots, characterized in that, include: The initialization module is used to define the state space of the vehicle-like robot; The global path planning module is used to plan the global path of the vehicle-like robot by performing a heuristic search in a continuous three-dimensional space using an improved hybrid A* algorithm. The trajectory fitting module is used to fit sparse state sequences based on global paths using Hermite curves to obtain time-space continuous trajectories. The collision model building module is used to model the space occupied by the vehicle-like robot and obstacles, and to establish a collision model between the vehicle-like robot and obstacles. An optimized obstacle avoidance control module is used to transform the geometric separation conditions between the robot and obstacles into a set of continuously differentiable nonlinear algebraic constraints based on a collision model and duality theory. Using the time-space continuous trajectory as a reference trajectory, a trajectory optimization objective function is constructed. This module can solve the motion control variables of the vehicle-like robot while satisfying the dynamic constraints, input constraints, and acceleration smoothing constraints of the vehicle-like robot, thus realizing obstacle avoidance planning and control of the vehicle-like robot for polygonal obstacles.
8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the motion optimization obstacle avoidance control method for vehicle-like robots as described in any one of claims 1-6.
9. A non-transitory computer-readable storage medium, characterized in that, The non-transitory computer-readable storage medium is used to store computer instructions, which, when executed by a processor, implement a motion optimization obstacle avoidance control method for vehicle-like robots as described in any one of claims 1-6.
10. An electronic device, characterized in that, include: The device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, and the computer program is stored in the memory. When the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to implement a motion optimization obstacle avoidance control method for a vehicle-like robot as described in any one of claims 1-6.