Space-time joint A-star based trajectory planning method and system for objects of arbitrary shape

Abstract

The application belongs to the field of robot motion planning and navigation technology, and provides an arbitrary shape object trajectory planning method and system based on space-time combined A-star, which comprises the following technical solutions: generating all discrete angle corresponding bounding boxes and robot contour mask information based on the constructed robot polygon model; defining a four-dimensional state space and initializing a starting state and a target state; in the four-dimensional state space, for the node with the minimum value, a predefined motion primitive is applied to generate a successor state candidate set; for each successor state candidate node, the discrete angle corresponding bounding box and robot contour mask information of the current node are extracted, and a bit operation is performed on the corresponding area of the environment grid map, and whether collision occurs is judged according to the operation result; when the search reaches the target state or meets the termination condition, a discrete space-time path is generated by backtracking, the discrete space-time path is interpolated and smoothed, and a final executable trajectory is output.

Description

Technical Field

[0001] This invention belongs to the field of robot motion planning and navigation technology, and particularly relates to a method and system for trajectory planning of arbitrary-shaped objects based on spatiotemporal joint A-satellite. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] With the rapid development of robotics technology, navigation and path planning have become core challenges in fields such as mobile robots, robotic arms, and continuum robots. Traditional path planning algorithms, such as sampling-based methods (e.g., PRM), ), search-based methods (such as , Traditional methods, including optimization-based methods (such as DWA and MPC), often simplify robots into regular shapes, such as circles, ellipses, or rectangles, when dealing with robot motion. While this simplification simplifies computation, it ignores the robot's actual complex geometry, leading to the loss of potential feasible solutions in narrow or dense environments. For example, for robots with non-convex polygonal profiles or internal holes (such as mobile platforms with robotic arms or flexible continuum robots), traditional methods significantly reduce planning flexibility and efficiency by narrowing the feasible region or approximating the shape.

[0004] Furthermore, traditional algorithms typically consider only the spatial dimension (such as two-dimensional or three-dimensional position and orientation), ignoring the temporal dimension. While this is acceptable in static environments, it can lead to infeasible or overly conservative planning results in dynamic environments (such as those with moving obstacles, multi-robot collaboration, or time window constraints). For example, in dynamic obstacle scenarios, the path may overlap with the obstacle's trajectory, causing collisions; in multi-robot collaboration, ignoring the time series can cause resource conflicts or synchronization problems. Some existing spatiotemporal planning methods, such as variants of spatiotemporal RRT or spatiotemporal... Although a time dimension is introduced, it is often limited to objects with simple shapes, and the collision detection efficiency is low, making it unable to process complex shapes in real time.

[0005] Collision detection is a bottleneck in path planning. Traditional methods rely on geometric intersection calculations (such as the GJK algorithm or SAT test), which are computationally expensive in real-time applications, especially when the object shape is complex. While raster-based methods are efficient, they require real-time recalculation for rotations and translations of arbitrary shapes, leading to increased latency. Pre-computed masks have been used in some fields (such as computer graphics), but have not yet been combined with spatiotemporal methods. Deep integration of algorithms. Summary of the Invention

[0006] To address at least one of the technical problems mentioned above, this invention provides a method and system for trajectory planning of arbitrary-shaped objects based on spatiotemporal joint A-satellite. The generated trajectory not only satisfies the kinematic constraints of the robot but also effectively reduces sharp turns in the path, improving the tracking accuracy and energy efficiency of the actuator.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] The first aspect of the present invention provides a method for trajectory planning of an arbitrary-shaped object based on a spatiotemporally coupled A-satellite, comprising the following steps:

[0009] Parametric modeling of an irregularly shaped robot yields a polygonal model of the robot. Based on the polygonal model of the robot, bounding boxes corresponding to all discrete angles and mask information of the robot contour are generated.

[0010] Define a four-dimensional state space that includes two-dimensional position, discrete orientation, and time dimension, and initialize the starting state and the target state;

[0011] In the four-dimensional state space, for the node with the minimum cost, a predefined set of motion primitives is applied to generate a candidate set of successor states.

[0012] For each candidate node of the successor state, extract the bounding box and the mask information of the robot contour corresponding to the discrete angle of the current node, and perform bit operations in the corresponding area of ​​the environmental grid map. Determine whether there is a collision based on the operation results.

[0013] When the search reaches the target state or meets the termination condition, a discrete spatiotemporal path is generated by backtracking, and the discrete spatiotemporal path is smoothed by interpolation using a parameterized curve to output the final executable trajectory.

[0014] Furthermore, the parametric modeling of the irregularly shaped robot to obtain a polygonal model includes: defining the robot's outer contour and all internal holes through an ordered set of vertex sequences; the robot's shape is a closed outer polygon. and polygons with zero or more internal holes They are composed of a set of ordered vertices, with the first and last vertices coinciding to form a closed polygon.

[0015] Further, the step of generating binary mask images of robot contours corresponding to all discrete angles based on the robot polygon model includes:

[0016] The robot's yaw angle range Evenly divided into N Given a discrete angle interval, iterate through all discrete angle indices and calculate the corresponding discrete yaw angle;

[0017] By combining discrete yaw angles, a rotation transformation is performed on the vertices of the robot's outer contour and all internal holes to obtain the rotated coordinates;

[0018] The rotated coordinates are converted to pixel scale at a specified map resolution, and the local bounding box at the current discrete yaw angle is calculated by combining the minimum value of the pixel coordinates; a binary mask image of the robot contour at this discrete angle is generated based on the local bounding box.

[0019] Furthermore, in the four-dimensional state space, for the node with the minimum cost, predefined motion primitives are applied to generate a candidate set of successor states, including: for propulsion motion primitives, successor coordinates and new angle indices need to be calculated; for rotation motion primitives, the position coordinates remain unchanged, and the angle index is updated; propulsion motion primitives include straight travel along the current heading, left-turn arc propulsion, and right-turn arc propulsion; rotation motion primitives include in-situ left turn and in-situ right turn.

[0020] Furthermore, the specific process of collision determination is as follows:

[0021] Based on the location of candidate child nodes and local bounding box information, the corresponding area of ​​the image is extracted from the environment raster map as the region of interest (ROI).

[0022] Perform a bitwise AND operation between the mask image corresponding to the angle of the candidate child node and the ROI;

[0023] The number of non-zero pixels in the bitwise AND result image is counted. If the number is greater than zero, a collision is determined to have occurred; otherwise, no collision is determined.

[0024] Furthermore, the step of interpolating and smoothing the discrete spatiotemporal path using parameterized curves includes:

[0025] For each pair of adjacent points in the discrete-time path, estimate the velocity and construct the velocity vector for each node, using adjacent nodes as the basis. , and its velocity vector , As boundary conditions, a local cubic Hermit spline is constructed for each segment; after obtaining the Hermit curve, several points are uniformly sampled on the local normalized parameters, the corresponding continuous positions are calculated, a high-resolution smooth trajectory point sequence is generated, and a continuous parameterized trajectory function is obtained.

[0026] Furthermore, the expression for the Hermit curve is:

[0027] ,

[0028] in, The connection position corresponding to the sampling point. Represents the local normalization parameter. For time intervals, Represents a node The velocity vector, Represents a node The velocity vector.

[0029] A second aspect of the present invention provides a trajectory planning system for arbitrary-shaped objects based on a spatiotemporally coupled A-satellite, comprising:

[0030] The irregular object modeling module is used to parametrically model irregularly shaped robots to obtain a polygonal model of the robot. Based on the polygonal model of the robot, it generates bounding boxes corresponding to all discrete angles and mask information of the robot contour.

[0031] The state candidate set generation module is used to define a four-dimensional state space containing two-dimensional position, discrete orientation, and time dimensions, and initialize the starting state and the target state. In the four-dimensional state space, for the node with the minimum cost, predefined motion primitives are applied to generate the successor state candidate set.

[0032] The collision detection module is used to extract the bounding box and robot contour mask information corresponding to the discrete angle of the current node for each subsequent state candidate node, and perform bit operations in the corresponding area of ​​the environmental grid map to determine whether a collision occurs based on the operation results.

[0033] The trajectory planning module is used to backtrack and generate a discrete spatiotemporal path when the search reaches the target state or meets the termination condition, and to interpolate and smooth the discrete spatiotemporal path using a parameterized curve to output the final executable trajectory.

[0034] A third aspect of the present invention provides a computer-readable storage medium.

[0035] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the above-described method for trajectory planning of arbitrary-shaped objects based on a spatiotemporally linked A-satellite.

[0036] A fourth aspect of the present invention provides a computer device.

[0037] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps in the above-described method for trajectory planning of arbitrary-shaped objects based on a spatiotemporally linked A-satellite.

[0038] Compared with the prior art, the beneficial effects of the present invention are:

[0039] This invention parametrically models arbitrary shapes, defining the object's outer contour and internal holes through vertex sequences. It supports concave polygons and structures with holes, avoiding the solution space loss caused by traditional simplification. In efficient bitwise collision detection, it utilizes bitwise AND operations to quickly determine collisions in local ROI regions, reducing the computational overhead across the entire image.

[0040] In the spatiotemporal joint planning process of this invention, nodes explicitly contain a time dimension, which can directly handle dynamic obstacles, time windows and temporal constraints, and is suitable for dynamic multi-machine environments.

[0041] After generating the original discrete trajectory, this invention introduces Hermite curve interpolation for smoothing, which not only significantly improves the stability and tracking accuracy of the robot during actual execution, but also effectively reduces mechanical vibration, motor load impact and energy consumption, providing an important guarantee for reliable autonomous navigation of objects of arbitrary shapes in complex environments.

[0042] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0043] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0044] Figure 1 This is a flowchart of the trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite provided in an embodiment of the present invention;

[0045] Figure 2 This is a schematic diagram of robot mask generation and ROI collision detection under different discrete angles provided in the embodiments of the present invention;

[0046] Figure 3 This is a demonstration diagram of the pose result of planning an irregular object provided in an embodiment of the present invention. Detailed Implementation

[0047] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0048] It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. 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 invention pertains.

[0049] 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 scope of exemplary embodiments according to the invention. 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.

[0050] This invention provides a method for trajectory planning of arbitrary-shaped objects based on a spatiotemporally joint A-scan. This method achieves spatiotemporally joint trajectory planning for arbitrary-shaped objects in a two-dimensional grid map. By introducing a time-dimensional node expansion and employing shape collision detection based on mask overlap, it achieves feasible path search considering both the dynamic environment and the object's shape. For parametric modeling of arbitrary shapes: the outer contour and internal holes of the object are defined through vertex sequences, supporting concave polygons and structures with holes, avoiding solution space loss caused by traditional simplification. Simultaneously, a time dimension is explicitly introduced into the state nodes, and a precise shape collision detection mechanism based on pre-computed masks is used to achieve high-precision trajectory planning for arbitrary polygons (including concave polygons and shapes with internal holes) in dynamic environments. Furthermore, to address the problem that discrete, polygonal paths generated by A-search do not conform to the kinematic continuity of the robot, after obtaining the discrete spatiotemporal path, a cubic Hermite interpolation curve is used to smooth the path. By introducing endpoint constraints for position and heading, and optionally curvature continuity constraints, a smooth trajectory with continuous position is generated. This trajectory not only satisfies the robot's kinematic constraints but also effectively reduces sharp turns in the path, improving the tracking accuracy and energy efficiency of the actuator.

[0051] Example 1

[0052] This embodiment provides a trajectory planning method for arbitrary-shaped objects based on a spatiotemporally combined A-satellite system, mainly composed of four parts: irregular object modeling, trajectory planning, collision detection, and trajectory smoothing. The irregular object modeling part uses discrete points of the object's outer contour to construct its contour polygon, and the internal disconnected regions can also be represented using the same method. To avoid performing expensive real-time rotation matrix operations during the search, the object contour is discretized and a mask is generated. Regarding trajectory planning, based on... The algorithm is improved by defining state nodes, designing motion primitives, and implementing reasonable costs to enhance search efficiency. In the collision detection section, pre-stored position masks and map ROIs from the modeling phase are used, and bitwise operations are employed to determine if a collision has occurred. Finally, if the target point is reached or the time limit is exceeded, trajectory backtracking is performed, and Hermite curves are applied to smooth the final trajectory planning result. The overall block diagram of this invention is shown below. Figure 1 As shown, it includes the following steps:

[0053] Step 1: Parametrically model the irregularly shaped robot to obtain a polygonal model of the robot, and generate bounding boxes and mask information of the robot contour corresponding to all discrete angles based on the polygonal model of the robot;

[0054] Specifically, the steps include the following:

[0055] Step 101: Perform parametric modeling on the irregularly shaped robot to obtain a polygonal model of the robot;

[0056] Specifically, the robot's outer contour and all internal holes are defined by an ordered set of vertex sequences. The robot's shape is a closed outer polygon. and polygons with zero or more internal holes Together constitute;

[0057] Each polygon is represented by an ordered list of vertices: The coordinates of each vertex The unit is meters, and the first and last vertices coincide to form a closed polygon.

[0058] The parametric modeling method described above can accurately describe arbitrary concave and convex polygonal shapes as well as complex geometric structures with internal holes.

[0059] For example, for a typical F-shaped robot, its outer contour can be defined as an irregular polygon with a central bulge, while internal holes represent cavities or non-solid areas in the design. This modeling method is highly flexible and can seamlessly adapt to various robot morphologies, including but not limited to mobile robotic arm bases, platforms with complex attachments, and flexible continuum robots. This parametric representation provides a reliable geometric foundation for subsequent angle discretization, mask pre-calculation, and accurate collision detection, ensuring high-fidelity shape representation and collision determination under any orientation.

[0060] Step 102: Generate bounding boxes corresponding to all discrete angles based on the robot polygon model, and at the same time generate mask information of the robot contour;

[0061] Specifically, the steps include the following:

[0062] Range of robot yaw angle The angle is uniformly divided into N discrete angle intervals, where N is a positive integer, for example... Or 36; Traverse all discrete angle indices Calculate the corresponding discrete yaw angle ;

[0063] Vertices of the robot's outer contour and all internal holes (Unit: meters), perform a rotation transformation to obtain the rotated coordinates. : .

[0064] Calculate the local bounding box at the current discrete yaw angle based on the pixel coordinates of all rotated vertices. ;in and These are the minimum pixel coordinates of all rotated vertices. and These are the width and height of the bounding box, respectively.

[0065] Create a binary mask image of the corresponding size in memory. (Data type is uint8), initial value is 0. The mask generation process is as follows:

[0066] Translate the coordinates of the rotated outer contour vertices to a point with the top left corner of the bounding box as the origin (i.e., subtract the coordinates of the outer contour vertices). ), to obtain the local coordinates relative to the bounding box;

[0067] The outer contour area is filled with 255 using a polygon filling algorithm to represent the area occupied by the robot entity;

[0068] For each internal hole, its vertex coordinates are also rotated and translated to the local coordinate system of the bounding box, and the corresponding polygon area is filled with a pixel value of 0 to achieve precise "sculpting" of the hole.

[0069] The final mask In the diagram, areas with a pixel value of 255 represent the space occupied by the robot entity in that pose, while areas with a pixel value of 0 represent free space (including internal holes in the design). Masks for all discrete angles are shown below. and its bounding box information These values ​​are uniformly stored in a pre-computation cache structure. This pre-computation process is completed once before path planning begins, allowing for direct and fast access via angle indexes during subsequent real-time search and collision detection stages. This avoids repeated rotation transformations and polygon rasterization operations, significantly reducing computational burden and improving the algorithm's real-time performance.

[0070] Step 2: Define a four-dimensional state space containing two-dimensional position, discrete orientation, and time dimensions, and initialize the starting state and the target state;

[0071] In this embodiment, the four-dimensional state space is represented as follows: ,in These are the raster pixel coordinates. Discrete orientation index (0 to ), For discrete time intervals, the initial state is... Target state ,in, and These represent the starting point and the target orientation, respectively.

[0072] Furthermore, it also includes initializing the priority queue OpenList and initializing the accessed collection ClosedSet;

[0073] The initialization priority queue OpenList pushes a tuple containing the starting value, timestamp, and state information into the queue and then sorts it according to the priority. f = g + h Sort; where, g The actual cumulative cost from the starting point to the present. h Estimate the cost to reach the target; initialize the accessed set ClosedSet (store) The mapping table ParentMap between the triplet and the parent node.

[0074] Step 3: In the four-dimensional state space, for the node with the minimum cost, apply predefined motion primitives to generate a candidate set of successor states;

[0075] In this embodiment, the process of predefining motion primitives includes increasing the time step by a fixed amount each time a node is expanded. The design incorporates five basic motion elements, including forward movement, curved turning, and in-situ rotation, as follows:

[0076] Motion Element 1: Proceed straight along the current course. ;

[0077] Motion Element 2: Turn left in place. ;

[0078] Motion Element 3: Turn right in place. ;

[0079] Motion Element 4: Left-turn arc propulsion, ;

[0080] Motion Element 5: Right Turn Arc Propulsion, ,in Indicates the translation step size. Indicates the rotation step size. k This represents the index of motion primitives.

[0081] These primitives ensure the continuity and flexibility of motion, while supporting non-omnidirectional robots. The expanded set of nodes is as follows: .

[0082] Specifically, motion primitives 1, 4, and 5 are propulsion types, including straight movement along the current heading, left-turn arc propulsion, and right-turn arc propulsion; calculate subsequent coordinates: New Angle Index ,in, Indicates the robot's current position. Indicates the current orientation angle. .

[0083] Motion primitives 2 and 3 are rotational; their position coordinates remain unchanged, but their angle indices are updated. ,in .

[0084] Calculate the new time index , Indicates the index at the current time, if Exceeding the preset maximum search time If the state is not found, the subsequent state is discarded to ensure time constraints are met.

[0085] If the priority queue OpenList is empty, then path planning is considered to have failed.

[0086] If the priority queue OpenList is not empty, pop the value from OpenList. f The smallest node is used as the current node. Determine the current node position With the target location Is the Euclidean distance less than a set threshold? And yaw angle residual Within the tolerance range, for example, <= 1, if so, recursively trace the source based on the parent node index stored in ParentMap, extract the physical parameters of each node, and construct a discrete spatiotemporal pathpoint sequence. The output is sent to the robot's actuator;

[0087] Specifically, value f The calculation formula is f = g + h , g The actual cumulative cost from the starting point to the present, including distance traveled, time, and penalties for changes in heading. h The estimated cost from the current node to the target is calculated using a weighted heuristic function that integrates the spatial Euclidean distance and yaw angle residuals. express:

[0088] ,

[0089] in, Let be the position of the previous node and the Euclidean distance. , Represents the angular residual. The angle residual weight.

[0090] Step 4: For each candidate node of the successor state, extract the mask and bounding box information corresponding to the discrete angle of the current node, and perform bit operations in the corresponding area of ​​the environmental grid map. Determine whether there is a collision based on the operation results.

[0091] In this embodiment, during the search process, for each candidate child node Extract the mask and bounding box information corresponding to the discrete angles of the current node from the cache. and In the environmental grid map In China, according to and Extracting the Region of Interest (ROI) (size and...) (Same) Perform pixel-level bitwise operations, i.e. Result = (Bitwise AND); Statistics Result Number of non-zero pixels Count ,like Result Non-zero pixel count Count If the value is greater than 0, a collision is determined, and the node is discarded; otherwise, no collision has occurred, and the incremental cost of this action is calculated, including the movement distance cost, time cost, and heading change cost. The heuristic cost of the successor node is then calculated. h ;

[0092] This method utilizes hardware acceleration for bitwise operations, making it far more efficient than geometric calculations. It also supports dynamic obstacles. If it is a time-varying map, then it can be... ROI is updated constantly.

[0093] Step 5: When the search reaches the target state or the termination condition is met, backtrack to generate a discrete spatiotemporal path, and use parameterized curves to interpolate and smooth the discrete spatiotemporal path to output the final executable trajectory.

[0094] After obtaining the discrete spatiotemporal path, to improve the continuity, executability, smoothness, and energy efficiency of the robot motion, this embodiment introduces cubic Hermite spline curves for post-processing interpolation. Hermite interpolation is a classic local interpolation method. By simultaneously specifying the endpoint positions of each curve segment and its first derivative (tangent vector), it can ensure that the entire trajectory achieves C¹ continuity in both position and velocity (first derivative), thereby effectively avoiding common problems in discrete paths such as sharp corners, sharp turns, or sudden velocity changes.

[0095] Specifically, the steps include the following:

[0096] Step 501: Backtrack to obtain the discrete spatiotemporal path;

[0097] Arbitrary trajectory points in a discrete-time path point sequence ,in, For pixel coordinates, This is the yaw angle index after discretization. For the corresponding discrete-time index, the path point sequence is used to calculate the velocity vector between adjacent points.

[0098] Step 502: Estimate the velocity of each pair of adjacent points in the discrete spatiotemporal path and define the tangent vector to provide the necessary first-order derivative information for subsequent Hermite interpolation;

[0099] For each pair of adjacent points and Velocity estimation is obtained using a position-time difference method:

[0100] ,

[0101] Construct the velocity vector for each node:

[0102] ,

[0103] in, The average speed is calculated by taking the average of two adjacent speed segments for internal nodes. The first and last nodes can be used directly. This ensures that adjacent Hermite segments have continuous tangent vectors at shared nodes (C¹ continuity).

[0104] Step 503: Using adjacent nodes , and its velocity vector , As boundary conditions, construct local cubic Hermite splines for each segment;

[0105] For the part( Introducing local normalization parameters Time interval The Hermit spline curve expression is obtained as follows:

[0106] .

[0107] Step 504: Uniform sampling generates the final high-resolution trajectory; after obtaining the Hermit curve, for each segment... ,exist A number of points are uniformly sampled (typically 10-30 sampling points), and the corresponding continuous positions are calculated. This generates a high-resolution, smooth trajectory point sequence. Ultimately, a continuous parameterized trajectory function is obtained. (Constructed by stitching together segmented Hermite data), it can be directly used by robot controllers (such as position servo or velocity / accelerometer feedforward control). The final generated trajectory is as follows: Figure 3 As shown.

[0108] At the same time, mask overlap detection is performed again on the sampling points. If a slight collision occurs, the magnitude of the tangent vector can be reduced (to reduce tension) or the local path can be reverted to the original discrete path.

[0109] Through the Hermite curve smoothing process described above, the original discrete spatiotemporal path is transformed into a smooth, velocity-continuous reference trajectory. This not only significantly improves the robot's stability and tracking accuracy during actual execution but also effectively reduces mechanical vibration, motor load impact, and energy consumption, providing crucial support for reliable autonomous navigation of objects of arbitrary shapes in complex environments. This step ensures a smooth trajectory, avoids sharp turns in the discrete path, and improves the robot's following accuracy and energy efficiency.

[0110] Example 2

[0111] This embodiment provides a trajectory planning system for arbitrary-shaped objects based on the spatiotemporal joint A-satellite, including:

[0112] The irregular object modeling module is used to parametrically model irregularly shaped robots to obtain a polygonal model of the robot. Based on the polygonal model of the robot, it generates bounding boxes corresponding to all discrete angles and mask information of the robot contour.

[0113] The state candidate set generation module is used to define a four-dimensional state space containing two-dimensional position, discrete orientation, and time dimensions, and initialize the starting state and the target state. In the four-dimensional state space, for the node with the minimum cost, predefined motion primitives are applied to generate the successor state candidate set.

[0114] The collision detection module is used to extract the bounding box and robot contour mask information corresponding to the discrete angle of the current node for each subsequent state candidate node, and perform bit operations in the corresponding area of ​​the environmental grid map to determine whether a collision occurs based on the operation results.

[0115] The trajectory planning module is used to backtrack and generate a discrete spatiotemporal path when the search reaches the target state or meets the termination condition, and to interpolate and smooth the discrete spatiotemporal path using a parameterized curve to output the final executable trajectory.

[0116] It should be noted that the specific implementation of the trajectory planning system for arbitrary-shaped objects based on spatiotemporal joint A-satellite in this embodiment of the invention is similar to the specific implementation of the trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite in this embodiment of the invention. For details, please refer to the description in the method section. To reduce redundancy, it will not be repeated here.

[0117] Example 3

[0118] This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps in the above-described method for trajectory planning of arbitrary-shaped objects based on spatiotemporal joint A-satellite.

[0119] Example 4

[0120] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps in the above-described method for trajectory planning of arbitrary-shaped objects based on spatiotemporal joint A-satellite.

[0121] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage and optical storage) containing computer-usable program code.

[0122] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart 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.

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

[0124] 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.

[0125] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0126] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite, characterized in that, include: Parametric modeling of an irregularly shaped robot yields a polygonal model of the robot. Based on the polygonal model of the robot, bounding boxes corresponding to all discrete angles and mask information of the robot contour are generated. Define a four-dimensional state space that includes two-dimensional position, discrete orientation, and time dimension, and initialize the starting state and the target state; In the four-dimensional state space, for the node with the minimum cost, a predefined set of motion primitives is applied to generate a candidate set of successor states. For each candidate node of the successor state, extract the bounding box and the mask information of the robot contour corresponding to the discrete angle of the current node, and perform bit operations in the corresponding area of ​​the environmental grid map. Determine whether there is a collision based on the operation results. When the search reaches the target state or meets the termination condition, a discrete spatiotemporal path is generated by backtracking, and the discrete spatiotemporal path is smoothed by interpolation using a parameterized curve to output the final executable trajectory. The step of using parametric curves to interpolate and smooth the discrete spatiotemporal path includes: For each pair of adjacent points in the discrete-time path, estimate the velocity and construct the velocity vector for each node, using adjacent nodes as the basis. , and its velocity vector , As boundary conditions, a local cubic Hermit spline is constructed for each segment; after obtaining the Hermit curve, several points are uniformly sampled on the local normalized parameters, the corresponding continuous positions are calculated, a high-resolution smooth trajectory point sequence is generated, and a continuous parameterized trajectory function is obtained. The expression for the Hermit curve is: ， in, The connection position corresponding to the sampling point. Represents the local normalization parameter. For time intervals, Represents a node The velocity vector, Represents a node The velocity vector.

2. The trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in claim 1, characterized in that, The parametric modeling of the irregularly shaped robot to obtain a polygonal model includes: defining the robot's outer contour and all internal holes through an ordered set of vertex sequences; the robot's shape is a closed outer polygon. and polygons with zero or more internal holes They are composed of a set of ordered vertices, with the first and last vertices coinciding to form a closed polygon.

3. The trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in claim 1, characterized in that, The generation of binary mask images of robot contours corresponding to all discrete angles based on the robot polygon model includes: The robot's yaw angle range Evenly divided into N Given a discrete angle interval, iterate through all discrete angle indices and calculate the corresponding discrete yaw angle; By combining discrete yaw angles, a rotation transformation is performed on the vertices of the robot's outer contour and all internal holes to obtain the rotated coordinates; The rotated coordinates are converted to pixel scale at a specified map resolution, and the local bounding box at the current discrete yaw angle is calculated by combining the minimum value of the pixel coordinates; a binary mask image of the robot contour at this discrete angle is generated based on the local bounding box.

4. The trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in claim 1, characterized in that, In the four-dimensional state space, for the node with the minimum cost, predefined motion primitives are applied to generate a candidate set of successor states. This includes: for propulsion motion primitives, successor coordinates and new angle indices need to be calculated; for rotation motion primitives, the position coordinates remain unchanged, and the angle index is updated. Propulsion motion primitives include straight travel along the current heading, left-turn arc propulsion, and right-turn arc propulsion; rotation motion primitives include in-situ left turn and in-situ right turn.

5. The trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in claim 3, characterized in that, The specific process for collision determination is as follows: Based on the location of candidate child nodes and local bounding box information, the corresponding area of ​​the image is extracted from the environment raster map as the region of interest (ROI). Perform a bitwise AND operation between the mask image corresponding to the angle of the candidate child node and the ROI; The number of non-zero pixels in the bitwise AND result image is counted. If the number is greater than zero, a collision is determined to have occurred; otherwise, no collision is determined.

6. A trajectory planning system for arbitrary-shaped objects based on spatiotemporal joint A-satellite, characterized in that, The method for planning the trajectory of an arbitrary-shaped object based on spatiotemporal joint A-satellite as described in any one of claims 1-5 includes: The irregular object modeling module is used to parametrically model irregularly shaped robots to obtain a polygonal model of the robot. Based on the polygonal model of the robot, it generates bounding boxes corresponding to all discrete angles and mask information of the robot contour. The state candidate set generation module is used to define a four-dimensional state space containing two-dimensional position, discrete orientation, and time dimensions, and initialize the starting state and the target state. In the four-dimensional state space, for the node with the minimum cost, predefined motion primitives are applied to generate the successor state candidate set. The collision detection module is used to extract the bounding box and robot contour mask information corresponding to the discrete angle of the current node for each subsequent state candidate node, and perform bit operations in the corresponding area of ​​the environmental grid map to determine whether a collision occurs based on the operation results. The trajectory planning module is used to backtrack and generate a discrete spatiotemporal path when the search reaches the target state or meets the termination condition, and to interpolate and smooth the discrete spatiotemporal path using a parameterized curve to output the final executable trajectory.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in any one of claims 1-5.

8. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the trajectory planning method for arbitrary-shaped objects based on spatiotemporal joint A-satellite as described in any one of claims 1-5.

Images