Orthophoto image navigation-oriented elastic safety constraint trajectory generation method and system

Abstract

The application discloses an elastic safety constraint trajectory generation method and system for orthographic image navigation, and relates to the technical field of mobile robot path planning. In view of the problems that the orthographic image has splicing errors, AI recognition leads to fuzzy edges of passable areas, and traditional grid expansion easily causes narrow passages to disappear, the application constructs a grid map by acquiring and preprocessing the orthographic image, and calculates the distance field value of each grid unit to the nearest obstacle. A safety risk penalty term negatively related to the distance field value is introduced in the A* algorithm to dynamically control the safety distance between the path point and the obstacle and generate a global path. The discrete path points are fitted into a time parameterized B-spline curve to generate dense trajectory points. The elastic safety constraint takes into account the passability and safety of the path, generates a smooth and trackable trajectory, improves the autonomous navigation ability and motion control precision of the robot in a complex environment, and can be widely applied to air-ground integrated rescue, field inspection and other scenes.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning technology, specifically to a method and system for generating flexible safety constraint trajectories for orthophoto navigation. Background Technology

[0002] With the rapid development of UAV photogrammetry and embodied intelligence technology, integrated air-ground rescue technology has brought new breakthroughs to traditional rescue methods. Using UAV digital orthophotos to guide ground mobile robots to target points has become an important research direction in this field. Digital orthophotos are geometrically corrected remote sensing images with accurate geographic coordinate information, providing ground robots with a global environmental map. Currently, robot navigation methods based on orthophotos are mainly divided into online and offline paradigms.

[0003] Online paradigms typically combine single-frame UAV digital orthophotos with image segmentation algorithms to locate moving objects on the ground and mark passable areas within a global view, thus planning barrier-free routes for robots. Offline paradigms, on the other hand, are geared towards specific operational scenarios, such as field power line inspections. They combine orthophotos and digital elevation models, adding cost terms such as the number of climbs and path tortuosity to the cost function to generate paths that suit the physical exertion of the workers.

[0004] However, the above methods still have many safety risks in practical applications. First, digital orthophotos have inherent errors during the stitching process, the size of which is directly related to the number of images acquired and the optimization time. Considering the timeliness requirements of data acquisition and processing at the rescue site, it is difficult to perform long-term fine optimization. The average positioning accuracy of points on the image is about 0.5 meters, which may lead to deviations between the actual and planned locations. Second, the recognition accuracy and generalization ability of AI semantic segmentation algorithms are limited, especially in unpaved road scenarios where the edges are not obvious. The boundaries of passable areas are often blurred and difficult to define precisely. Finally, existing path planning algorithms usually perform obstacle inflation processing on the global grid map to ensure the safe passage of robots, that is, artificially expanding the obstacle area to reserve a safe buffer distance. Although this conservative approach improves safety, it also causes some narrow passages that are originally passable to be completely covered and misjudged as impassable areas, putting the robot in a "path that cannot be traveled" dilemma, reducing the passability and success rate of the mission.

[0005] The aforementioned issues make it difficult to simultaneously ensure navigation safety and maneuverability in complex environments when planning paths for ground mobile robots on orthophotos. This problem is particularly prominent in scenarios with high requirements for timeliness and reliability, such as rescue operations. Summary of the Invention

[0006] This invention provides a method and system for generating flexible safety-constrained trajectories for orthophoto navigation, aiming to solve the incompatibility problem between planning passability and safety caused by the fuzzy labeling of AI algorithms under orthophotos. This enables robots to achieve long-endurance autonomous rescue missions under complex road conditions with both structured and unstructured elements. At the path planning level, precise nearest obstacle distance calculation replaces the coarse cell expansion strategy. By designing a safety heuristic with flexible constraints, prior distance field information is embedded into the A* neighborhood search process, constructing an integrated framework for global path optimization and flexible local safety distance control. At the trajectory generation level, coarse grid path points are fitted into time-parameterized B-spline curves to generate travel trajectory points that can be used for precise time and position control.

[0007] On the one hand, a method for generating flexible safety constraint trajectories for orthophoto navigation is provided, including:

[0008] Obtain orthophotos of the area to be planned, and preprocess the orthophotos to construct a grid map containing passable areas and obstacle areas;

[0009] Calculate the distance field value of each grid cell in the grid map, and construct a security risk penalty item based on the distance field value;

[0010] Perform path search, and introduce the security risk penalty term into the priority calculation to obtain a global path containing discrete path points;

[0011] The discrete path points are fitted into a time-parameterized B-spline curve to generate dense trajectory points.

[0012] Further, the orthophoto image is preprocessed, specifically including:

[0013] Obtain orthophotos and their corresponding geographic coordinate boundary information, and crop the orthophotos to extract the region of interest;

[0014] A semantic segmentation network is used to perform pixel-level classification of the region of interest to identify passable areas and obstacle areas.

[0015] The classification results are mapped back to the original image coordinate system and rasterized at a preset resolution to construct the raster map.

[0016] Furthermore, the distance field value is the distance from each grid cell to the nearest obstacle.

[0017] Furthermore, the calculation method for the security risk penalty item is as follows:

[0018] ;

[0019] in, The cell where the current position is located. for A cell in the neighborhood, For the guiding term weight, for The distance field value, The user's desired security threshold, for The set of neighboring cells.

[0020] Furthermore, the path search is implemented using an improved A* algorithm, and the priority of each cell is calculated as follows:

[0021] ;

[0022] in, This indicates the cell containing the starting point. The cell representing the endpoint. This indicates the cell where the current position is located. express A cell in the neighborhood, Indicates starting from the origin Go to current location The historical optimal cost This represents the cost of movement between adjacent cells. As a safety risk penalty item, Indicates from cell To the finish line The heuristic term is calculated as follows:

[0023] ;

[0024] in, , These are cells , Offset in two coordinate directions.

[0025] Further, the discrete path points are fitted into a time-parameterized B-spline curve to generate dense trajectory points, including:

[0026] Calculate the center position of each grid cell in the global path, construct a B-spline curve equation with 3 degrees of freedom, and perform interpolation based on the center position as the position control point to obtain continuous dense trajectory points.

[0027] Furthermore, after generating the dense trajectory points, the process also includes an overshooting step for the beginning and end regions of the trajectory:

[0028] Detect whether the velocity control point and acceleration control point exceed the preset kinematic constraints;

[0029] For control points that exceed speed constraints, the speed is compressed by extending the corresponding node vector interval.

[0030] For control points that exceed acceleration constraints, the acceleration value is compressed by extending the corresponding node vector interval.

[0031] Prioritize adjusting the speed control point, then adjust the acceleration control point to ensure that the trajectory meets the robot's speed and acceleration limits.

[0032] Furthermore, before performing path search, a connectivity verification step between the starting point and the target point is also included:

[0033] Transform the GPS coordinates of the starting point and the target point to a raster coordinate system and calculate the corresponding raster index;

[0034] A depth-first search algorithm is used to mark connected components in passable regions;

[0035] If the starting point and the target point are in the same connected component, then proceed with the subsequent path search; otherwise, it indicates that they are not connected.

[0036] On the other hand, a flexible safety constraint trajectory generation system for orthophoto navigation is provided, comprising:

[0037] The preprocessing module is configured to: acquire orthophotos of the area to be planned, preprocess the orthophotos to construct a grid map containing passable areas and obstacle areas; calculate the distance field value of each grid cell in the grid map, and construct a safety risk penalty item based on the distance field value;

[0038] The global path planning module is configured to: perform path search, introduce the security risk penalty term in priority calculation, and obtain a global path containing discrete path points;

[0039] The trajectory generation module is configured to fit the discrete path points into a time-parameterized B-spline curve to generate dense trajectory points.

[0040] In another aspect, a computer-readable storage medium is also provided, on which a computer program is stored, which, when executed by a processor, performs the method described in the first aspect.

[0041] The above technical solution has the following advantages or beneficial effects:

[0042] (1) This invention replaces the traditional grid expansion processing strategy with a precise nearest obstacle distance field calculation, fundamentally avoiding the problem of narrow passable areas being misjudged as obstacles and disappearing due to conservative expansion. By introducing a safety risk penalty term with negative correlation characteristics into the neighborhood search process of the A* algorithm, dynamic and elastic control of the safe distance between path points and obstacles is achieved. In wide road areas, the algorithm can guide the robot to walk above the preset safe distance, ensuring sufficient safety redundancy; while in narrow passage areas, when the expected safety threshold cannot be reached, the algorithm automatically guides the robot to pass through the safest area along the center line of the passage. This elastic adaptability enables the robot to ensure the optimality of the global path and take into account the safety of local passage in complex environments that simultaneously contain structured roads and unstructured paths, effectively solving the technical problem of incompatibility between planning passability and safety caused by AI label ambiguity under orthophotos.

[0043] (2) This invention fits the discrete grid path points obtained from global path planning into a time-parameterized cubic B-spline curve to generate continuous dense trajectory points. Compared with the traditional method of directly using the grid center point as the path point, the trajectory generation method of this invention ensures the smoothness of the trajectory itself and its velocity and acceleration, avoiding violent turning or sudden velocity changes during robot movement. At the same time, by performing dynamic constraint overshoot processing on the beginning and end regions of the trajectory, it ensures that the generated trajectory strictly meets the robot's velocity and acceleration limits, making the trajectory practically executable and significantly improving the robot's travel stability and control accuracy.

[0044] (3) This invention introduces fault tolerance for map uncertainties at the path planning level by constructing a global distance field and designing elastic safety constraints based on it. Even if there is a stitching error of about 0.5 meters in the orthophoto, or if the edge of the passable area is blurred due to AI recognition, the algorithm can still use the distance field information to perceive the actual width of the local passable space and adaptively adjust the path position to guide the robot to a safer area. This elastic constraint mechanism based on geometric distance reduces the over-reliance on the precise boundary of the semantic segmentation result, effectively improves the robustness of the entire navigation system in the face of image reconstruction errors and AI recognition generalization problems, and ensures the robot's long-endurance autonomous task execution capability in complex rescue environments. Attached Figure Description

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

[0046] Figure 1 This is a schematic diagram of orthophoto reconstruction error in Embodiment 1 of the present invention;

[0047] Figure 2 This is a schematic diagram illustrating the AI ​​algorithm's recognition error in Embodiment 1 of the present invention;

[0048] Figure 3 This is a flowchart of the method according to Embodiment 1 of the present invention;

[0049] Figure 4 This is a schematic diagram of the overall technical solution framework of Embodiment 1 of the present invention;

[0050] Figure 5 This is a schematic diagram of the distance field calculation results according to Embodiment 1 of the present invention;

[0051] Figure 6 This is a schematic diagram illustrating the connected component determination in Embodiment 1 of the present invention;

[0052] Figure 7 This is a schematic diagram of the planning result of the traditional A* algorithm in Embodiment 1 of the present invention;

[0053] Figure 8 This is a schematic diagram of the planning result of the improved A* algorithm in Embodiment 1 of the present invention;

[0054] Figure 9 This is a schematic diagram of the long-distance planning effect of Embodiment 1 of the present invention when the grid resolution is 0.5m;

[0055] Figure 10 This is a schematic diagram of the long-distance planning effect of Embodiment 1 of the present invention when the grid resolution is 1m;

[0056] Figure 11 This is a schematic diagram illustrating the effect of global path planning and time parameterized trajectory generation in Embodiment 1 of the present invention. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings. Those skilled in the art should understand that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0058] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration 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.

[0059] Example 1

[0060] Currently, planning the path of a ground mobile robot on digital orthophotos often presents numerous safety hazards, and its shortcomings mainly lie in the following three aspects:

[0061] (1) Errors exist in the stitching process of digital orthophotos. In engineering, the magnitude of the error is directly related to the number of acquisitions and the optimization time. Considering the timeliness requirements of acquisition and processing at the rescue site, the average positioning accuracy of points on the image is about 0.5m. Figure 1 As shown.

[0062] (2) AI recognition accuracy and generalization issues lead to blurred edges of passable areas, especially for unpaved roads with indistinct side edges, such as Figure 2 As shown.

[0063] (3) Path planning algorithms usually expand the global grid to improve security, which causes some narrow passable areas to face the risk of disappearing.

[0064] To address the above shortcomings, this invention proposes a flexible safety constraint design and trajectory generation method for orthophoto navigation, aiming to solve the problem of incompatibility between planning passability and safety caused by the fuzzy marking of AI algorithms under orthophotos, enabling robots to achieve long-endurance autonomous rescue missions under complex road conditions with both structured and unstructured features.

[0065] This embodiment provides a method for generating flexible safety constraint trajectories for orthophoto navigation. Figure 3 This is a flowchart of a method for generating elastic safety constraint trajectories for orthophoto-based navigation according to an embodiment of the present invention, as shown below. Figure 3 As shown, the method includes the following steps:

[0066] S101: Obtain orthophotos of the area to be planned, preprocess the orthophotos, and construct a grid map containing passable areas and obstacle areas;

[0067] S102: Calculate the distance field value of each grid cell in the raster map, and construct a security risk penalty item based on the distance field value;

[0068] S103: Perform path search, introduce a security risk penalty term in the priority calculation, and obtain a global path containing discrete path points;

[0069] S104: Fit discrete path points into time-parameterized B-spline curves to generate dense trajectory points.

[0070] Figure 4 This is a schematic diagram of the overall technical solution framework of Embodiment 1 of the present invention, as shown below. Figure 4 As shown below. Each step will be explained in detail below.

[0071] In step S101, the orthophoto image is preprocessed, including:

[0072] First, acquire the orthophoto of the area to be planned. The orthophoto is a GeoTIFF file embedding geographic coordinate information. Then, read the coordinate reference system (CRS) and affine transformation parameters of the orthophoto, and construct a coordinate transformer from WGS84 to GeoTIFFCRS (e.g., UTM50N). Finally, transform all boundary vertices of the planning area from the WGS84 coordinate system to the UTM50N projected coordinate system, and perform cropping of the orthophoto in the projected coordinate system to remove non-imaged areas and retain the ROI image.

[0073] On the cropped ROI image, a pre-trained mask2former semantic segmentation network is used to predict pixel categories, identifying passable areas (such as roads and grasslands) and obstacle areas (such as buildings and water bodies). The window position (row-offset, column-offset, height, width) of the ROI in the original GeoTIFF is recorded simultaneously during ROI cropping. Therefore, the recognition results within the ROI can be pasted back into the entire image, outputting pixel-level semantic categories for the entire image, as well as metadata including projection parameters, origin coordinates, and corner WGS84 information, for precise geolocation in subsequent steps.

[0074] A raster map in meters is constructed based on the projected coordinate boundaries of GeoTIFF, and the raster resolution is defined as [value missing]. Depending on the specific application scenario, the resolution can be selected as 0.25m, 0.5m, or 1.0m. After determining the raster origin and the number of rows and columns, each raster cell is mapped to an image pixel window, and the proportion of valid pixels in the window belonging to the passable semantic label is counted. If this proportion exceeds a preset threshold (e.g., 50%), it is determined to be a passable cell (Free); otherwise, it is an obstacle cell (Occ). Finally, a binary raster image (0, 1) with passability labels is output, where 0 represents passability and 1 represents an obstacle, along with raster metadata containing projection, origin coordinates, and corner WGS84 information.

[0075] In S102, the distance field value of each grid cell in the raster map is calculated, and a security risk penalty term is constructed based on the distance field value, including:

[0076] The distance field value is defined as the Euclidean distance from each grid cell to the nearest obstacle. The input to the global distance field calculation is a binarized grid map with Free / Occ tags obtained in step S101, and the output is a distance field grid map.

[0077] To improve computational efficiency, this embodiment employs the traveling parabola method for efficient distance transformation, specifically as follows:

[0078] First, calculate the range field of the one-dimensional grid. Using the notation... To represent a one-dimensional grid, its cells are represented by... Representation, symbol Indicates distance transformation, To retrieve the value currently stored in the cell, then:

[0079] (1);

[0080] Where p represents the cell from which the distance transformation is to be calculated, It is a binary function, defined as:

[0081] (2);

[0082] Calculate the cell using formula (1). In the parabola All vertical coordinates Take the minimum value as the cell value. The distance transformation results are obtained. Directly performing the distance transformation is computationally complex; therefore, this embodiment employs the parabolic traverse method to improve computational efficiency. Specifically, based on the one-dimensional distance field algorithm, the distance field of each cell is calculated row-wise first, and then column-wise, to obtain the distance field of the two-dimensional grid.

[0083] Figure 5 An example of distance field calculation results is shown, where the numbers in the white grid represent the nearest distance to the surrounding black grid (obstacles).

[0084] Subsequently, a security risk penalty term is constructed based on the distance field value:

[0085] (3);

[0086] in, The cell where the current position is located. for A cell in the neighborhood, for m Neighboring cell set The weight of the guiding term indicates that the higher the weight, the faster the response speed of the safety regulation. for The distance field value, The security threshold expected by the user.

[0087] Understandably, the penalty term is negatively correlated with the distance field value; that is, the safer the cell (the larger the distance field value), the smaller the penalty term value.

[0088] In S103, path search is performed, and a security risk penalty term is introduced into the priority calculation to obtain a global path containing discrete path points, including:

[0089] This embodiment, based on the traditional A* algorithm, introduces a safety risk penalty term with flexible constraint characteristics to the node priority during the search process, forming an improved priority expression:

[0090] (4);

[0091] in, This indicates the cell containing the starting point. The cell representing the endpoint. This indicates the cell where the current position is located. express A cell in the neighborhood, Indicates starting from the origin Go to current location The historical optimal cost This represents the cost of movement between adjacent cells. Indicates from cell To the finish line The heuristic term is calculated using the diagonal distance method:

[0092] (5);

[0093] in, , These are cell g and cell g, respectively. Offset in two coordinate directions.

[0094] The security risk penalty term obtained in step S102 is introduced to link node priority with security: that is, the more secure the node (the larger the distance field value), the smaller the penalty term and the higher the priority.

[0095] Specifically, during algorithm execution, each time starting from the current position... An 8-neighborhood search yields feasible regions. The priority of each neighboring node is calculated according to formula (4). And select the node with the highest priority (i.e. P(n) The smallest node is used as the next expansion node. The above expansion process is repeated until the target node g is expanded, and a global path containing discrete path points from the starting point s to the ending point g is obtained.

[0096] The search process has the following flexible control characteristics: when there is a distance field value in the neighborhood that is greater than or equal to the safety threshold... When the distance field value of all neighboring nodes is less than zero, the penalty term tends to zero, and the algorithm moves towards the shortest path; when the distance field value of all neighboring nodes is less than zero, the penalty term tends to zero, and the algorithm moves towards the shortest path. At that time, the penalty term guides the algorithm to move towards the node with the largest distance field value in the neighborhood (i.e. the safest region), thereby achieving flexible safety control and effectively solving the problem of incompatibility between planning passability and safety caused by the blurring of AI algorithm labels under orthophotos.

[0097] Before performing a path search, connectivity checks must be performed on the starting and target regions to ensure that a feasible path exists, including:

[0098] First, convert the GPS coordinates of the current and target locations to the projected coordinate system. Then, calculate the cell index based on the resolution of the raster coordinate system.

[0099] , (6);

[0100] in,( Represents the position components in the projected coordinate system. This represents the coordinates of the top-left corner vertex of the orthophoto image in the projected coordinate system. () represents the cell index. This indicates the raster resolution, and floor(.) indicates rounding down.

[0101] Connectivity determination, such as Figure 6 As shown, depth-first search or breadth-first search algorithms are used to label the connected components of the passable region. Figure 4 White cells represent passable areas, gray cells represent obstacle areas, blue arrows indicate forward exploration, and red arrows indicate backtracking. If the starting point 's' and the target point... If the points are located within the same connected component, continue the path search; otherwise, it indicates that there is no connected path between the two points.

[0102] In S104, discrete path points are fitted into time-parameterized B-spline curves to generate dense trajectory points, including:

[0103] To obtain a smooth trajectory that satisfies dynamic constraints, the discrete path points generated in step S103 are fitted into time-parameterized cubic uniform B-spline curves. Fitting all path points into time-parameterized B-spline curves can provide dense path points for the ground mobile robot, improving the smoothness of travel and control accuracy.

[0104] Specifically, first, the center position of each grid cell in the global path is calculated as the interpolation point of the B-spline curve:

[0105] , (7);

[0106] This embodiment uses a uniform B-spline curve with 3 degrees of freedom. This represents the trajectory of a ground-based mobile robot.

[0107] (8);

[0108] in, This indicates the number of path points that need to be traversed. Indicates the location control point. For control point index variables, This represents a B-spline basis function with 3 degrees of freedom.

[0109] and They are The first and second derivatives can be linearly expressed by control points, defined as follows: , , They are , The first node vector There are n nodes, and they satisfy:

[0110] (9);

[0111] Based on the properties of uniform B-spline curves, interpolation... Each path point needs Define one location control point. Sequence of location control points Then any node in the effective interval Position, velocity, and acceleration can all be determined by three consecutive control points. , express:

[0112] (10);

[0113] (11);

[0114] (12);

[0115] in, These are the velocity control point and the acceleration control point. For uniform B-spline curve node vector spacing, velocity control points The relationship with the location control points is as follows:

[0116] (13);

[0117] Acceleration control point The relationship with the location control points is as follows:

[0118] (14);

[0119] according to Based on the velocity and acceleration values ​​at each of the path points and the four start and end times, a system can be established. A linear equation, control points These are the unknowns in the equation. A point in space has three dimensions, and the components in each dimension need to be calculated separately. Using... express If a certain dimension component is given, then the number of variables is... The system of linear equations can be written in matrix form as follows:

[0120] (15);

[0121] (16);

[0122] (17);

[0123] (18);

[0124] Where C represents the coefficient matrix of the linear equation system, the first K rows correspond to the equation coefficients of formula (10). The last 4 rows correspond to the equation coefficients of formulas (11) and (12). x represents all control points, the first K elements of b represent the K positions to be interpolated, and the last 4 elements represent the starting velocity, starting acceleration, ending velocity, and ending acceleration to be interpolated.

[0125] Solving the system of linear equations will yield the control points of the B-spline curve.

[0126] After obtaining the control points, a sampling interval is set to generate dense trajectory points. Sampling interval Based on the robot's maximum speed Distance to adjacent control points determined:

[0127] (19);

[0128] in, The operator represents the Euclidean distance. Then, by substituting into formula (8), the dense trajectory points from the starting point to the target point can be obtained. The dense trajectory point sequence is then input into the underlying controller of the ground mobile robot as a reference trajectory.

[0129] Optionally, in this embodiment, the underlying controller may employ the Model Predictive Control (MPC) method. Specifically, the MPC controller uses the robot's current pose as the initial state and the trajectory point as the tracking target, driving the robot to smoothly travel along the trajectory until it reaches the target point.

[0130] To further ensure the feasibility of the trajectory, this embodiment also performs dynamic constraint overshoot processing on the beginning and end regions of the trajectory. Since the speed of the mobile robot is usually zero at the start and end points, calculating the sampling interval according to formula (19) on the equidistant grid will result in a large acceleration near the start and end points, exceeding the robot's dynamic constraints. Therefore, it is necessary to adjust the sampling interval near the start and end points separately.

[0131] This embodiment uses the following method to adjust and process the overshoot in the beginning and end regions of the trajectory:

[0132] By utilizing the convex hull property of B-splines, and by detecting and adjusting velocity and acceleration control points that exceed the constraints, it can be ensured that the velocity and acceleration at any point on the entire trajectory satisfy the motion constraints. The specific process is as follows:

[0133] definition Speed ​​control point One component that does not satisfy the velocity constraint. The maximum speed at which the robot can move is its absolute value component. Further definition Substituting it into formula (13) yields the adjusted speed control point components. As shown in formula (21):

[0134] (twenty one);

[0135] By extending the node vector from arrive The length of the compression point can be determined to achieve the compression process of the speed control point.

[0136] Similarly, definition It is the acceleration control point A component that does not satisfy the acceleration constraint, The absolute value component of the maximum acceleration that allows the robot to move. Further definition Substituting this into formula (14) yields the adjusted acceleration control point components. As shown in formula (22)

[0137] (twenty two);

[0138] By extending the node vector from arrive The acceleration control point can be lowered by adjusting the length of the curve.

[0139] The specific adjustment sequence is as follows: first adjust the velocity control point, then adjust the acceleration control point. If a point in the node vector is extended, then subsequent nodes should be moved forward by the same amount, resulting in an overall time that only increases and never decreases. Reducing acceleration also reduces velocity, therefore, there will be no velocity rebound issue during the acceleration adjustment process.

[0140] Through the above steps, this embodiment generates a dense trajectory that satisfies safety, smoothness, and dynamic constraints, realizing autonomous navigation that balances passability and safety in complex environments.

[0141] like Figure 7-8 As shown, the global path planning algorithm proposed in this invention exhibits significant resilience and adaptability in terms of both throughput and safety. Figure 7-8 The diagrams show the path planning results of the traditional A* algorithm and the improved A* algorithm proposed in this invention. It can be seen that in wide-road scenarios, the algorithm proposed in this invention can guide the robot to always maintain a preset safe distance (e.g., 2.5 meters) or more, ensuring sufficient safety redundancy. In narrow-channel scenarios, when the width of the passable area cannot meet the safety threshold requirement, the algorithm automatically adjusts its strategy, guiding the robot to pass through the safest area along the centerline of the channel, thus maximizing the passability of narrow areas while ensuring basic safety. This flexible adjustment mechanism effectively solves the problem of narrow channels disappearing due to fixed expansion processing in traditional methods.

[0142] Figure 9-10 The paper demonstrates the long-distance planning performance of the present invention at different grid resolutions, verifying the applicability and stability of the algorithm on multi-scale maps. Regardless of whether a resolution of 0.5m or 1.0m is used, the algorithm can generate continuous and reasonable global paths, demonstrating its good adaptability to map accuracy.

[0143] Figure 11 This demonstrates the combined effect of global path planning and time-parameterized trajectory generation, where red dots represent discrete path points and blue curves represent the generated smooth trajectories. It is evident that by fitting discrete path points to B-spline curves, this invention obtains continuous and dense trajectory points that satisfy dynamic constraints, providing a foundation for high-precision tracking control of robots.

[0144] In summary, the elastic safety constraint design and trajectory generation method for orthophoto navigation proposed in this invention breaks through the limitation of the inability to flexibly control the boundary safety of path planning under traditional digital orthophotos. This enables ground mobile robots to achieve long-distance autonomous rescue missions under complex road conditions including structured roads and unstructured paths, significantly improving the environmental adaptability and mission success rate of the navigation system.

[0145] Example 2

[0146] This embodiment provides a flexible safety constraint trajectory generation system for orthophoto navigation, including:

[0147] The preprocessing module is configured to: acquire orthophotos of the area to be planned, preprocess the orthophotos to construct a grid map containing passable areas and obstacle areas; calculate the distance field value of each grid cell in the grid map, and construct a safety risk penalty item based on the distance field value;

[0148] The global path planning module is configured to: perform path search, introduce a security risk penalty term in priority calculation, and obtain a global path containing discrete path points;

[0149] The trajectory generation module is configured to fit discrete path points into time-parameterized B-spline curves to generate dense trajectory points.

[0150] It should be noted that the above modules are the same as the examples and application scenarios implemented in the corresponding steps of Embodiment 1, but are not limited to the content disclosed in Embodiment 1. It should also be noted that the above modules, as part of a system, can be executed in a computer system such as a set of computer-executable instructions.

[0151] The descriptions of each embodiment in the above embodiments have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0152] The proposed system can be implemented in other ways. For example, the system embodiments described above are merely illustrative, and the division of modules described above is only a logical functional division. In actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated into another system, or some features may be ignored or not executed.

[0153] Example 3

[0154] This embodiment also provides a computer-readable storage medium for storing computer instructions, which, when executed by a processor, complete the method described in Embodiment 1.

[0155] 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 method for generating elastic safety constraint trajectories for orthophoto navigation, characterized in that, include: Obtain orthophotos of the area to be planned, and preprocess the orthophotos to construct a grid map containing passable areas and obstacle areas; Calculate the distance field value of each grid cell in the grid map, and construct a security risk penalty item based on the distance field value; The calculation method for the security risk penalty item is as follows: ； in, The cell where the current position is located. for A cell in the neighborhood, For the weight of the guiding term, for The distance field value, The user's desired security threshold, for The set of neighboring cells; Perform path search, and introduce the security risk penalty term into the priority calculation to obtain a global path containing discrete path points; The path search is implemented using an improved A* algorithm, and the priority of each cell is calculated as follows: ； in, This indicates the cell containing the starting point. The cell representing the endpoint. This indicates the cell where the current position is located. express A cell in the neighborhood, Indicates starting from the origin Go to current location The historical optimal cost This represents the cost of movement between adjacent cells. As a safety risk penalty item, Indicates from cell To the finish line The heuristic term is calculated as follows: ； in, , These are cells , Offset in two coordinate directions; The discrete path points are fitted into a time-parameterized B-spline curve to generate dense trajectory points.

2. The method for generating elastic safety constraint trajectories for orthophoto-based navigation according to claim 1, characterized in that, The orthophoto image is preprocessed, specifically including: Obtain orthophotos and their corresponding geographic coordinate boundary information, and crop the orthophotos to extract the region of interest; A semantic segmentation network is used to perform pixel-level classification of the region of interest to identify passable areas and obstacle areas. The classification results are mapped back to the original image coordinate system and rasterized at a preset resolution to construct the raster map.

3. The method for generating elastic safety constraint trajectories for orthophoto-based navigation according to claim 1, characterized in that, The distance field value is the distance from each grid cell to the nearest obstacle.

4. The method for generating elastic safety constraint trajectories for orthophoto-based navigation according to claim 1, characterized in that, The discrete path points are fitted into a time-parameterized B-spline curve to generate dense trajectory points, including: Calculate the center position of each grid cell in the global path, construct a B-spline curve equation with 3 degrees of freedom, and perform interpolation based on the center position as the position control point to obtain continuous dense trajectory points.

5. The method for generating elastic safety constraint trajectories for orthophoto-based navigation according to claim 4, characterized in that, After generating the dense trajectory points, the process also includes an overshoot processing step for the beginning and end regions of the trajectory: Detect whether the velocity control point and acceleration control point exceed the preset kinematic constraints; For control points that exceed speed constraints, the speed is compressed by extending the corresponding node vector interval. For control points that exceed acceleration constraints, the acceleration value is compressed by extending the corresponding node vector interval. Prioritize adjusting the speed control point, then adjust the acceleration control point to ensure that the trajectory meets the robot's speed and acceleration limits.

6. The method for generating elastic safety constraint trajectories for orthophoto-based navigation according to claim 1, characterized in that, Before performing path search, a connectivity check between the starting point and the target point is also included: Transform the GPS coordinates of the starting point and the target point to a raster coordinate system and calculate the corresponding raster index; A depth-first search algorithm is used to mark connected components in passable regions; If the starting point and the target point are in the same connected component, then proceed with the subsequent path search; otherwise, it indicates that they are not connected.

7. A flexible safety constraint trajectory generation system for orthophoto navigation, characterized in that, include: The preprocessing module is configured to: acquire orthophotos of the area to be planned, preprocess the orthophotos to construct a grid map containing passable areas and obstacle areas; calculate the distance field value of each grid cell in the grid map, and construct a safety risk penalty item based on the distance field value; The calculation method for the security risk penalty item is as follows: ； in, The cell where the current position is located. for A cell in the neighborhood, For the weight of the guiding term, for The distance field value, The user's desired security threshold, for The set of neighboring cells; The global path planning module is configured to: perform path search, introduce the security risk penalty term in priority calculation, and obtain a global path containing discrete path points; The path search is implemented using an improved A* algorithm, and the priority of each cell is calculated as follows: ； in, This indicates the cell containing the starting point. The cell representing the endpoint. This indicates the cell where the current position is located. express A cell in the neighborhood, Indicates starting from the origin Go to current location The historical optimal cost This represents the cost of movement between adjacent cells. As a safety risk penalty item, Indicates from cell To the finish line The heuristic term is calculated as follows: ； in, , These are cells , Offset in two coordinate directions; The trajectory generation module is configured to fit the discrete path points into a time-parameterized B-spline curve to generate dense trajectory points.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the method for generating flexible safety constraint trajectories for orthophoto-based navigation as described in any one of claims 1-6.

Images