An autonomous navigation system and method in a complex three-dimensional scene

Abstract

The present application belongs to the technical field of autonomous navigation, and particularly relates to a kind of full autonomous navigation system and method under complex three-dimensional scene.The present application extracts the central axis based on the obstacle distance field to construct the passable corridor, without relying on the semantic information of environment, can quickly plan out the global path that meets the kinematic constraint, greatly reduces the calculation complexity of path search under complex three-dimensional scene.With the help of global reference path, the index path is generated, and then the local sampling is carried out around the index path to construct the undirected graph to search the collision-free trajectory, which can reduce the local trajectory search range, improve the calculation efficiency of trajectory planning, and ensure the safety of the planning result.Through the rolling horizon optimization to solve the control instruction, the deviation caused by global planning and environment perception can be corrected in time, the stability and robustness of navigation process are improved, the full autonomous navigation under three-dimensional scene with complex obstacle distribution and large terrain fluctuation can be realized, and it is suitable for various mobile platforms.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation technology, specifically relating to a fully autonomous navigation system and method in complex three-dimensional scenes. Background Technology

[0002] Currently, autonomous navigation technologies in complex 3D scenes can be mainly divided into three categories: 1. Global path planning methods based on graph search (such as A*, Dijkstar, RRT, etc.): can search for feasible paths in known or partially known environments, but usually assume that the environment is static, and the planned paths often have many polyline segments and poor smoothness, making them difficult to use directly for tracking mobile platforms with severe dynamic constraints.

[0003] 2. Local trajectory planning methods based on sampling optimization (such as dynamic window method and gradient optimization method): can perform real-time obstacle avoidance, but lack global vision, are prone to getting trapped in local optima, and are prone to planning failure in narrow and complex channels.

[0004] 3. Learning-based end-to-end navigation methods: rely on training with a large amount of data, have weak generalization ability, poor interpretability, and insufficient reliability in safety-critical scenarios.

[0005] The main drawback is: 1. Disconnect between global and local planning: The global path lacks consideration of dynamic obstacles and platform movement capabilities, and local planning is prone to deviating from the global goal; 2. Poor environmental adaptability: In complex three-dimensional unstructured scenarios (such as dense buildings, forests, and underground caves), traditional single planning strategies are difficult to balance efficiency, safety, and smoothness simultaneously. 3. Difficulty in balancing real-time performance and accuracy: When dynamic obstacles frequently appear, replanning takes a long time, and the tracking and control links lag, leading to trajectory deviation or collision risks. Summary of the Invention

[0006] The purpose of this invention is to provide a fully autonomous navigation system and method for complex three-dimensional scenes, which can generate passable corridors globally and ensure the feasibility of the path in terms of kinematics and dynamics, perform real-time obstacle avoidance planning locally while maintaining consistency with the global path, and achieve high accuracy of trajectory, smooth tracking and closed-loop control at the control end.

[0007] The specific technical solution adopted by this invention is as follows: A fully autonomous navigation method for complex 3D scenes includes: The heading angle and velocity of the unmanned vehicle are discretized and sampled in a three-dimensional environment space, and a set of candidate paths is generated by polynomial fitting. The central axis is extracted based on the obstacle distance field to construct a pipe-shaped passable corridor. Within the passable corridor, a global reference path that satisfies the kinematic constraints is selected from the candidate path set. Guided by the global reference path, an index path is generated based on the current location and the target location on the global path; Select sampling points around the index path with a set step size and width, construct an undirected graph by connecting the sampling points, remove edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. Using the collision-free local execution trajectory as a reference, the system solves for the control command sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period. The first command in the control command sequence is then applied to the actuator to complete the navigation task of the current control cycle.

[0008] In a preferred embodiment, the candidate path set is obtained by discretizing the heading angle and velocity of each sampling point and generating several smooth curves connecting the current state and the next state using fifth-order polynomial fitting.

[0009] In a preferred embodiment, the accessible corridor is constructed as follows: Calculate the distance from each point in the 3D environment space to the nearest obstacle and generate a distance transformation map; Find the point with zero gradient in the distance transformation graph to form the central axis, and expand outwards from the central axis to the nearest obstacle to form a pipe-shaped safe zone; The boundaries of the passable corridors are determined by equidistant lines in the distance transformation diagram.

[0010] In a preferred embodiment, the step of selecting the global reference path that satisfies the kinematic constraints includes: The path cost of each candidate path in the candidate path set is evaluated by pre-setting initial weights, and a feasible path is obtained based on the path cost as the initial solution. The initial weights are gradually reduced and recorded as iterative weights. The cost of the candidate path set is re-evaluated and searched based on the iterative weights to obtain the iterative path as the iterative solution. After each iteration solution is generated, the real-time output of the iteration solution is compared with the previous iteration solution; If the path cost corresponding to the real-time output iterative solution is less than the path cost corresponding to the previous iterative solution, then the iterative solution is retained as the current optimal path, and the iteration continues. If the path cost corresponding to the real-time output iterative solution is not less than the path cost corresponding to the previous iterative solution, it indicates that the current optimal path has converged, the iteration stops, and the previous iterative solution is used as the global reference path for the final output.

[0011] In a preferred embodiment, the index path is a reference path formed by linearly interpolating several intermediate interpolation points based on the current position and the tracking target position intercepted on the global path.

[0012] In a preferred embodiment, the step of constructing the undirected graph includes: Based on the index path, expand to both sides with a set width along the direction perpendicular to the path, and evenly distribute sampling points along the path with a set step size, and add all sampling points as vertices to the vertex set. Calculate the straight-line distance between each newly added sampling point and other vertices in the vertex set, and mark vertices whose distance is less than a preset distance threshold as the neighborhood points of the corresponding sampling point; Connect the sampling points with all neighboring points using line segments to form the edges of an undirected graph; By repeatedly performing neighborhood point determination and edge connection operations on each sampling point until all sampling points have been processed, a complete undirected graph is obtained.

[0013] In a preferred embodiment, the step of removing edges and vertices that cross obstacles includes: In the constructed undirected graph, an initial candidate path is queried based on the start and end points, and each edge in the initial candidate path is checked to see if it collides with an obstacle. If an edge collides with an obstacle, then the edge that collides with the obstacle and the vertex associated with the edge that collides with the obstacle are removed from the undirected graph. In the deleted undirected graph, new candidate paths are queried again, and collision checks are performed again until all edges in the new candidate paths do not collide with obstacles. This is then used as the final local execution trajectory output.

[0014] In a preferred embodiment, the step of generating the control instruction sequence includes: Obtain the current state information and read the reference state and reference control instructions for each moment in the prediction period from the local execution trajectory; The motion relationship is simplified to a linear relationship in the vicinity of the reference state and reference control command; Based on linear relationships, an optimization index is established that includes a weighted sum of state deviation, control deviation, and control change amplitude. Upper and lower limits of control amplitude, control change rate, and safe range constraints of position and speed are also added. The minimum value of the optimization index under given constraints is obtained to obtain the control command sequence within the prediction period.

[0015] In a preferred embodiment, the state deviation is the difference between the predicted position at each moment within the prediction period and the reference position at the corresponding moment on the local execution trajectory; Control deviation is the difference between the predictive control command at each time point within the prediction period and the reference control command at the corresponding time point on the local execution trajectory; The control change amplitude is the difference between the predictive control commands at two adjacent moments within the prediction period.

[0016] This invention also provides a fully autonomous navigation system for complex 3D scenes, using the aforementioned fully autonomous navigation method for complex 3D scenes, comprising: The initialization module is used to discretize the heading angle and velocity of the unmanned vehicle in the three-dimensional environment space, and generate a set of candidate paths through polynomial fitting. The global planning module is used to extract the central axis based on the obstacle distance field to construct a pipe-shaped traversable corridor, and to select a global reference path that meets the kinematic constraints from the candidate path set within the traversable corridor. The index generation module is used to generate an index path based on the current position and the target position on the global path, guided by the global reference path. The trajectory planning module is used to select sampling points around the index path according to a set step size and width, construct an undirected graph by connecting the sampling points, remove edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. The control execution module is used to solve for the control instruction sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period, with reference to the collision-free local execution trajectory. The module then applies the first instruction in the control instruction sequence to the actuator to complete the navigation task of the current control cycle.

[0017] The technical effects achieved by this invention are as follows: This invention constructs a passable corridor by extracting the central axis based on the obstacle distance field. It can quickly plan a global path that satisfies kinematic constraints without relying on semantic information of the environment, significantly reducing the computational complexity of path search in complex 3D scenes. By generating a guiding index path using a global reference path, and then constructing an undirected graph to search for collision-free trajectories around the index path through local sampling, the local trajectory search range can be narrowed, improving the computational efficiency of trajectory planning while ensuring the safety of the planning results. By optimizing the control commands in the rolling temporal domain, deviations caused by global planning and environmental perception can be corrected in a timely manner, improving the stability and robustness of the navigation process. This enables fully autonomous navigation in 3D scenes with complex obstacle distribution and significant terrain undulations, and is adaptable to various mobile platforms. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the method flow of the present invention; Figure 2 This is a schematic diagram of the system modules of the present invention; Figure 3 This is a schematic diagram of the voronoi skeleton of the method of the present invention; Figure 4 This is a schematic diagram comparing the method of this invention with other global search algorithms; Detailed Implementation

[0019] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0020] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0021] Secondly, the term "an embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in a preferred embodiment" appearing in different places throughout this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that mutually excludes other embodiments.

[0022] Please see Figure 1 As shown, this invention provides a fully autonomous navigation method for complex 3D scenes, including: S1. Discretize and sample the heading angle and velocity of the unmanned vehicle in the three-dimensional environment space, and generate a set of candidate paths through polynomial fitting; In step S1, the candidate path set is obtained by discretizing the heading angle and velocity of each sampling point and using a fifth-order polynomial to generate several smooth curves connecting the current state and the next state. Specifically, sampling points are first set in the three-dimensional state space (position, velocity, heading). At each sampling point (x, y), the heading angle θ and velocity v are discretized, and each node is a state (x, y, θ, v). For the current state node s_start=(x1, y1, θ1, v1), the candidate state nodes s_next=(x2, y2, θ2, v2) in the next sampling layer are fitted with fifth-order polynomials to fit the vehicle's trajectory in the x and y directions respectively. ; ; Where t∈[0,T], and T is the planning time for the current state node to move to the next candidate state node. Boundary conditions are set based on the positions, heading angles, and velocities of the starting and ending nodes: ; ; ; ; Where x(t) and y(t) are position functions; , It is the derivative of the position function, i.e., the velocity function.

[0023] If the vehicle's acceleration is set to a fixed value 'a' according to actual operational requirements, then the acceleration constraint function can be obtained: ; ; By solving for the coefficients a0-a5 and b0-b5 of the fifth-order polynomial based on the above boundary conditions, a smooth candidate path connecting the current state node and the next candidate state node can be obtained.

[0024] S2. Extract the central axis based on the obstacle distance field to construct a pipe-shaped passable corridor, and select a global reference path that satisfies the kinematic constraints from the candidate path set within the passable corridor; In step S2, within the passable corridor, the central axis extracted using the Voronoi diagram is used as the path skeleton. Combined with the safe area generated by the obstacle distance field, collision detection and kinematic feasibility verification are performed on the candidate path set. Paths intersecting with obstacles or exceeding the turning and speed limits of the autonomous vehicle are eliminated, while paths that meet the requirements of curvature continuity and speed smoothness are retained, forming the final global reference path. The passable corridor is constructed as follows: Calculate the distance from each point in the 3D environment space to the nearest obstacle and generate a distance transformation map; Find the point with zero gradient in the distance transformation graph to form the central axis, and expand outwards from the central axis to the nearest obstacle to form a pipe-shaped safe zone; The boundaries of the passable corridors are determined by equidistant lines in the distance transformation diagram; Specifically, the state sampling points of Lattice are used as reference points in the three-dimensional workspace. Within this framework, the distances from each reference point to the nearest obstacle are calculated, forming a 3D distance transformation map. Let the set of obstacle points in the environment be: ; in, o iIndicates the first i One obstacle point, N This represents the total number of obstacle points. For any point in three-dimensional space: ; Definition point x To the obstacle collection O The distance transformation value is: ; in, D ( x ) represents a point x Distance to the nearest obstacle point d ( x , o i ) represents a point x With obstacle points o i The Euclidean distance between them is calculated using the following formula: ; in,( o ix ,o iy ,o iz ) represents the obstacle point o i The three-dimensional coordinates.

[0025] Based on the above set of obstacle points O Voronoi diagram is defined as: ; Where V represents the point set corresponding to the Voronoi diagram, and Ω represents the three-dimensional workspace. x Represents any point in the workspace. o i , o j Represents the set of obstacles O Two different obstacle points, i , j For obstacle point index, and i ≠ j ;d(x, o i ) and d(x, o j ) represent points respectively x To the obstacle point o i and o j The Euclidean distance, D( x ) represents a point xTo the obstacle collection O To find the minimum distance, find the point where the gradient of the 3D distance transformation map is zero and generate a central axis. Using this central axis as the center, calculate the distance to the nearest obstacle at the current point. Expanding to both sides forms a pipe-shaped safe zone, which is a passable corridor. The boundary of the passable corridor is determined by equidistant lines in the distance transformation diagram, ensuring that the path is always outside the safe distance of obstacles.

[0026] Secondly, the steps for selecting global reference paths that satisfy kinematic constraints include: The path cost of each candidate path in the candidate path set is evaluated by pre-setting initial weights, and a feasible path is obtained based on the path cost as the initial solution. The initial weights are gradually reduced and recorded as iterative weights. The cost of the candidate path set is re-evaluated and searched based on the iterative weights to obtain the iterative path as the iterative solution. After each iteration solution is generated, the real-time output of the iteration solution is compared with the previous iteration solution; If the path cost corresponding to the real-time output iterative solution is less than the path cost corresponding to the previous iterative solution, then the iterative solution is retained as the current optimal path, and the iteration continues. If the path cost corresponding to the real-time output iterative solution is not less than the path cost corresponding to the previous iterative solution, it indicates that the current optimal path has converged, the iteration stops, and the previous iterative solution is used as the global reference path for the final output. In the above, when evaluating the cost of candidate paths, the ARA* (AnytimeRepairingA*) algorithm is used for path search within the safe corridor. The ARA* algorithm is a variant of the A* algorithm, used to solve the shortest path problem. The ARA* algorithm introduces a heuristic inflation coefficient ε based on the heuristic cost function of the A* algorithm. The formula for calculating the heuristic total cost of node n is: ; in, f ε ( n ) represents a node n The overall search cost, g ( n () represents the distance from the starting node to the current node n The actual cumulative cost, h ( n ) represents the current node n Heuristic cost estimation to the target node ε Let represent the inflation coefficient of the heuristic function, and ε ≥1, in the initial search phase, the heuristic expansion coefficient is set to a large initial value, for example... ε0=3, to increase the cost of heuristic estimation. h ( n The influence of the heuristic expansion coefficient on the overall search cost of nodes causes the search process to prioritize expansion towards the target node, thereby reducing the search range of invalid nodes and obtaining an initial feasible path that satisfies safety and vehicle kinematic constraints in a shorter time. Subsequently, based on the obtained initial feasible path, the heuristic expansion coefficient is gradually reduced. ε, For example, by gradually decreasing the preset step size by 0.5 (i.e., successively taking values ​​of 3, 2.5, 2, 1.5, and 1), a suboptimal solution is found within a finite time. This solution is then gradually optimized in subsequent iterations. After each search, it checks if a better solution has been found; if so, the weights are updated. The algorithm stops and exits when no better solution is found after another search. Specifically, in this case, the algorithm approaches the optimal solution by gradually reducing the weights, while utilizing previously searched state information to avoid redundant calculations and improve search efficiency. In each iteration, the ARA* algorithm maintains a priority queue to store nodes to be expanded and calculates the heuristic cost based on the current weights. When the iteration weights decrease, the heuristic cost of the nodes in the priority queue is re-evaluated, and the queue is expanded again, gradually approaching a better iterative solution. This incremental optimization strategy ensures rapid generation of the initial solution and gradually improves the path quality through multiple iterations, making the final output global reference path as close as possible to the optimal solution while satisfying safety and kinematic constraints.

[0027] S3. Using the global reference path as a guide, generate an index path based on the current position and the target position on the global path; In step S3, after the global reference path is determined, it is used as a guide to generate an index path by combining the current autonomous vehicle pose with the target point on the global path. This facilitates subsequent local path planning and tracking control. The index path is a reference path formed by linear interpolation to calculate several intermediate interpolation points based on the current position and the target position captured on the global path. The specific steps are as follows: First, the nearest path point on the global reference path is found based on the current autonomous vehicle pose as the index starting point. Then, the corresponding path point on the global reference path is captured based on the set tracking distance as the index ending point. Finally, between the index starting point and the index ending point, a series of continuous intermediate interpolation points are generated using a linear interpolation method with equal intervals or equal time intervals. These interpolation points together constitute the index path, providing a corresponding tracking reference for subsequent local path planning.

[0028] S4. Select sampling points around the index path according to the set step size and width, construct an undirected graph by connecting the sampling points, remove the edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. In step S4, after the index path is determined, a local search space is constructed based on it to generate a collision-free local execution trajectory, thereby enabling the autonomous vehicle to drive safely in complex environments. The step of constructing the undirected graph includes: Based on the index path, expand to both sides with a set width along the direction perpendicular to the path, and evenly distribute sampling points along the path with a set step size, and add all sampling points as vertices to the vertex set. Calculate the straight-line distance between each newly added sampling point and other vertices in the vertex set, and mark vertices whose distance is less than a preset distance threshold as the neighborhood points of the corresponding sampling point; Connect the sampling points with all neighboring points using line segments to form the edges of an undirected graph; By repeatedly performing neighborhood point determination and edge connection operations on each sampling point until all sampling points have been processed, a complete undirected graph is obtained. Specifically, the purpose of constructing the undirected graph is to connect discrete sampling points around the index path into a graph structure through neighborhood relationships, thereby providing a topological foundation for subsequent path search. When constructing the undirected graph, the graph is uniformly expanded vertically with a set width centered on the index path. Multiple cross-sections are obtained by cutting the space along the index path with a fixed step size. A set number of sampling points are uniformly arranged on each cross-section. After all sampling points are added to the vertex set in sequence, neighborhood relationship determination is completed by using a distance threshold to avoid computational redundancy caused by full graph connection, while ensuring that the graph structure can cover a sufficient search space. After the undirected graph is constructed, collision detection is performed on all edges in the graph one by one to remove all edges that cross obstacles and isolated vertices that are completely connected by collision edges. The remaining connected undirected graph is the usable local search topology. Then, the A* algorithm is used to search from the vertex at the current position to the vertex at the target position, and the connected vertex sequence with the minimum total cost is selected. After connecting the vertices in sequence, a smooth collision-free local execution trajectory is obtained for subsequent control command solving.

[0029] Secondly, the steps to remove edges and vertices that cross obstacles include: In the constructed undirected graph, an initial candidate path is queried based on the start and end points, and each edge in the initial candidate path is checked to see if it collides with an obstacle. If an edge collides with an obstacle, then the edge that collides with the obstacle and the vertex associated with the edge that collides with the obstacle are removed from the undirected graph. In the deleted undirected graph, new candidate paths are queried again, and collision checks are performed again until all edges in the new candidate paths do not collide with obstacles. This is then used as the final local execution trajectory output. In the above process, the determination of collision with obstacles involves uniformly discretely sampling each edge in the constructed undirected graph. It is then determined whether the distance from each discrete sampling point to the nearest obstacle is greater than a preset safe distance threshold. If any discrete sampling point is less than or equal to the preset safe distance threshold, the edge is determined to have collided with the obstacle. Only when all discrete sampling points of an edge meet the distance requirement is the edge retained. After collision removal, all path segments in the remaining connected graph are in safe passable areas. The A* algorithm is then used to search for the vertex sequence with the minimum total cost. After connecting the vertex sequences, a smooth, collision-free local execution trajectory can be obtained through polynomial fitting.

[0030] S5. Using the collision-free local execution trajectory as a reference, solve for the control command sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period, and apply the first command in the control command sequence to the actuator to complete the navigation task of the current control cycle. In step S5, after the collision-free local execution trajectory is determined, the system state is continuously optimized within the prediction period according to the required prediction time length, ultimately forming an optimal control sequence. Based on this, the control command for the current moment is solved, enabling the autonomous vehicle to drive smoothly along the reference trajectory while effectively avoiding obstacles. The steps for generating the control command sequence include: Obtain the current state information and read the reference state and reference control instructions for each moment in the prediction period from the local execution trajectory; The motion relationship is simplified to a linear relationship in the vicinity of the reference state and reference control command; Based on linear relationships, an optimization index is established that includes a weighted sum of state deviation, control deviation, and control change amplitude. Upper and lower limits of control amplitude, control change rate, and safe range constraints of position and speed are also added. The minimum value of the optimization index under given constraints is obtained to obtain the control command sequence within the prediction period.

[0031] In this implementation, when generating the control command sequence, the current position, velocity, and heading angle of the unmanned vehicle are first obtained as initial values ​​for optimization. Then, based on the reference state given by each pre-aiming point on the local execution trajectory, the initial deviation between the current state and the reference state is calculated. Subsequently, the unmanned vehicle is linearized near the reference trajectory to obtain an incremental state transition equation, which is used as a constraint condition for the optimization problem. Corresponding weights are set for state deviation, control deviation, and control change amplitude in the optimization index. State deviation is the difference between the predicted position at each moment within the prediction period and the reference position at the corresponding moment on the local execution trajectory. Control deviation is the predicted control command at each moment within the prediction period. The difference between the control change amplitude and the reference control command at the corresponding moment on the local execution trajectory is the difference between the predicted control commands at two adjacent moments within the prediction period. In addition, physical constraints such as upper and lower limits of steering angle amplitude, driving speed amplitude, steering angular velocity, and acceleration are added, as well as safety range constraints such as the position not exceeding the boundary of the passable corridor and the speed not exceeding the limit value. Finally, the constrained optimization problem is solved by a quadratic programming algorithm to obtain the control command sequence in the entire prediction time domain. The first control command in the sequence is taken and output to the underlying execution mechanism of the autonomous vehicle. In the next control cycle, the complete process from S1 to S5 above is re-executed to complete the fully autonomous navigation in the rolling time domain.

[0032] Based on the above, the complete process of fully autonomous navigation is as follows: First, the position of the target being tracked (xe, ye, ze) is extracted from the global path. Then, the current position (xs, ys, zs) of the autonomous vehicle is obtained by subscribing to the localization TF sent by the SLAM algorithm. Since the autonomous vehicle can only move along the x and y directions on the ground, the difference between the z-axis and the target is ignored. The distance between the autonomous vehicle and the target being tracked is then calculated. Calculate the difference points based on the distance. Where step is the interpolation step size, and the position (xi, yi, zi) of each interpolation point is calculated based on the number of interpolation points. , , , After obtaining the target reference path, sampling points are selected around the reference path according to the set step size and width. The sampling points are connected with line segments according to the set criteria. In the graph constructed by the sampling points and the connections, the path from the starting point to the target point is queried. If the path found contains line segments located inside obstacles, the edges and vertices that cross obstacles in the planned route are deleted in the probabilistic path graph. The path is then queried again in the newly obtained probabilistic path graph. If the obtained path still contains edges and vertices that cross obstacles, the above process is repeated until the obtained path does not contain edges that cross obstacles, and the planning ends.

[0033] Scrolling optimization: ; in, For location, As a global reference point, To control the input, This is the weight matrix. This is the collision penalty coefficient; Dynamic obstacle avoidance constraints: Introducing obstacle avoidance constraints based on spatiotemporal occupancy prediction to support real-time avoidance of moving obstacles.

[0034] The specific steps are as follows: Generate reference path: Generate a reference path using linear interpolation based on the current position of the autonomous vehicle and the target position captured on the global path; Probabilistic route graph initialization: Initialize an empty undirected graph G(P, S), where P represents the set of vertices and S represents the set of collision-free edges (connections between vertices). Both P and S are initially empty. Spatial sampling: Sample multiple path points Pi from the environment space near the reference path according to specific rules, and add them to the vertex set P; Neighborhood point calculation: Define distance d. For a point that already exists in the vertex set P, if its distance to Pi is less than d, it is called a neighbor point of point Pi. Edge connection: Connect point Pi to its neighboring points to generate a set of lines s, until the number of points in the vertex set P reaches the set value, then the sampling phase ends; Path query: Query feasible paths from the constructed undirected graph G(P,S) based on the target point location and check whether the path collides with obstacles. If no collision occurs, output the path as the execution path.

[0035] Model predictive control tracking: Using the trajectory output from local planning as a reference input, high-precision closed-loop tracking is achieved through MPC.

[0036] Autonomous vehicle state vector: x = [X, Y, ψ, v] T; in: X,Y: The position (m) of the rear axle center of the unmanned vehicle in the world coordinate system; ψ: Heading angle (yaw angle, rad) of the unmanned vehicle; v : Longitudinal speed of the unmanned vehicle (m / s); Control input vector: u=[a,δ]T; in: a: Longitudinal acceleration (m / s²); δ: Front wheel steering angle (rad); Autonomous vehicle parameters: Lf: Distance from the front axle to the center of mass (m); Lr: Distance from the rear axle to the center of mass (m); L = Lf + Lr: Wheelbase (m); Functional expression of the continuous-time dynamics model:

[0037] in, It is the centroid sideslip angle; Discretized model (forward Euler method, sampling time) ): ; make This represents the discrete nonlinear function; At each control time t, MPC solves the following finite-time optimal control problem: Optimize variables: ; in, N To predict the length of the time domain.

[0038] Cost function: ; Where: xk|t: the state predicted at time t+k at time t; : The reference state and control input sequence from the local planner (RPM); (definition (This refers to the actual control input from the previous moment). Q > 0, R > 0, S > 0: State error, control error, and control increment weight matrix; P > 0: Terminal weight matrix, which is usually obtained by solving the algebraic Riccati equation to ensure stability.

[0039] Constraints: Dynamic constraints: ; in, This indicates the current measurement status.

[0040] Control input constraints (actuator physical limitations): ; Controlling incremental constraints (ensuring smoothness): ; State constraints (safety and physical limitations): Speed ​​constraints: ; Position constraints: ,in, Free space corridors provided for the local planner; Heading angle change rate constraint: ; Terminal constraints (optional, to enhance stability): ; in, The terminal constraint set is typically designed to be an invariant set.

[0041] For example: State acquisition and initialization (time t); The current state x(t) of the autonomous vehicle is obtained through sensors and a state estimator; Receive the reference trajectory from the local planner (RPM): ; Set the previous control input: ; Linearization and convexization (real-time iteration): Due to the nonlinearity of the model, it is usually in the current reference trajectory Linearization is performed in the vicinity: ; in: ;

[0042] This transforms the nonlinear MPC into a sequential quadratic programming (SQP) problem, with each SQP iteration solving a convex QP subproblem.

[0043] Solve the optimization problem: Solve the following quadratic programming problem: ; Here, ε is a slack variable, and ρ is a large penalty coefficient to ensure that the constraints are satisfied as much as possible.

[0044] Application control and scrolling: Obtain the optimal solution ; Apply only the first control input: ; Send u(t) to the underlying actuator (throttle / brake, steering system) and wait for the next sampling time t+1→t.

[0045] Please see Figure 2 A fully autonomous navigation system for complex 3D scenes, using the aforementioned fully autonomous navigation method for complex 3D scenes, includes: The initialization module is used to discretize the heading angle and velocity of the unmanned vehicle in the three-dimensional environment space, and generate a set of candidate paths through polynomial fitting. The global planning module is used to extract the central axis based on the obstacle distance field to construct a pipe-shaped traversable corridor, and to select a global reference path that meets the kinematic constraints from the candidate path set within the traversable corridor. The index generation module is used to generate an index path based on the current position and the target position on the global path, guided by the global reference path. The trajectory planning module is used to select sampling points around the index path according to a set step size and width, construct an undirected graph by connecting the sampling points, remove edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. The control execution module is used to solve for the control instruction sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period, with reference to the collision-free local execution trajectory. The module then applies the first instruction in the control instruction sequence to the actuator to complete the navigation task of the current control cycle.

[0046] The execution process of the navigation system described above corresponds to the process of the aforementioned method, and will not be repeated here.

[0047] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. Structures, devices, and operating methods not specifically described or explained in this invention are implemented according to conventional methods in the art unless otherwise specified or limited.

Claims

1. A fully autonomous navigation method for complex 3D scenes, characterized in that: include: Discretize the heading angle and velocity of the unmanned vehicle in a three-dimensional environment space, and generate a set of candidate paths through polynomial fitting; The central axis is extracted based on the obstacle distance field to construct a pipe-shaped passable corridor. Within the passable corridor, a global reference path that satisfies the kinematic constraints is selected from the candidate path set. Guided by the global reference path, an index path is generated based on the current location and the target location on the global path; Select sampling points around the index path with a set step size and width, construct an undirected graph by connecting the sampling points, remove edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. Using the collision-free local execution trajectory as a reference, the system solves for the control command sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period. The first command in the control command sequence is then applied to the actuator to complete the navigation task of the current control cycle.

2. The fully autonomous navigation method in a complex 3D scene according to claim 1, characterized in that: The candidate path set is obtained by discretizing the heading angle and velocity of each sampling point and using a fifth-order polynomial fitting to generate several smooth curves connecting the current state and the next state.

3. The fully autonomous navigation method in a complex 3D scene according to claim 1, characterized in that: The accessible corridor is constructed as follows: Calculate the distance from each point in the 3D environment space to the nearest obstacle and generate a distance transformation map; Find the point with zero gradient in the distance transformation graph to form the central axis, and expand outwards from the central axis to the nearest obstacle to form a pipe-shaped safe zone; The boundaries of the passable corridors are determined by equidistant lines in the distance transformation diagram.

4. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The step of selecting global reference paths that satisfy kinematic constraints includes: The path cost of each candidate path in the candidate path set is evaluated by pre-setting initial weights, and a feasible path is obtained based on the path cost as the initial solution. The initial weights are gradually reduced and recorded as iterative weights. The cost of the candidate path set is re-evaluated and searched based on the iterative weights to obtain the iterative path as the iterative solution. After each iteration solution is generated, the real-time output of the iteration solution is compared with the previous iteration solution; If the path cost corresponding to the real-time output iterative solution is less than the path cost corresponding to the previous iterative solution, then the iterative solution is retained as the current optimal path, and the iteration continues. If the path cost corresponding to the real-time output iterative solution is not less than the path cost corresponding to the previous iterative solution, it indicates that the current optimal path has converged, the iteration stops, and the previous iterative solution is used as the global reference path for the final output.

5. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The index path is a reference path formed by linearly interpolating several intermediate interpolation points based on the current position and the tracking target position captured on the global path.

6. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The steps for constructing the undirected graph include: Based on the index path, expand to both sides with a set width along the direction perpendicular to the path, and evenly distribute sampling points along the path with a set step size, and add all sampling points as vertices to the vertex set. Calculate the straight-line distance between each newly added sampling point and other vertices in the vertex set, and mark vertices whose distance is less than a preset distance threshold as the neighborhood points of the corresponding sampling point; Connect the sampling points with all neighboring points using line segments to form the edges of an undirected graph; By repeatedly performing neighborhood point determination and edge connection operations on each sampling point until all sampling points have been processed, a complete undirected graph is obtained.

7. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The step of removing edges and vertices that cross obstacles includes: In the constructed undirected graph, an initial candidate path is queried based on the start and end points, and each edge in the initial candidate path is checked to see if it collides with an obstacle. If an edge collides with an obstacle, then the edge that collides with the obstacle and the vertex associated with the edge that collides with the obstacle are removed from the undirected graph. In the deleted undirected graph, new candidate paths are queried again, and collision checks are performed again until all edges in the new candidate paths do not collide with obstacles. This is then used as the final local execution trajectory output.

8. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The step of generating the control command sequence includes: Obtain the current state information and read the reference state and reference control instructions for each moment in the prediction period from the local execution trajectory; The motion relationship is simplified to a linear relationship in the vicinity of the reference state and reference control command; Based on linear relationships, an optimization index is established that includes a weighted sum of state deviation, control deviation, and control change amplitude. Upper and lower limits of control amplitude, control change rate, and safe range constraints of position and speed are also added. The minimum value of the optimization index under given constraints is obtained to obtain the control command sequence within the prediction period.

9. The fully autonomous navigation method in a complex three-dimensional scene according to claim 1, characterized in that: The state deviation is the difference between the predicted position at each moment within the prediction period and the reference position at the corresponding moment on the local execution trajectory; Control deviation is the difference between the predictive control command at each time point within the prediction period and the reference control command at the corresponding time point on the local execution trajectory; The control change amplitude is the difference between the predictive control commands at two adjacent moments within the prediction period.

10. A fully autonomous navigation system for complex three-dimensional scenes, characterized in that: The fully autonomous navigation method for complex 3D scenes according to any one of claims 1 to 9 includes: The initialization module is used to discretize the heading angle and velocity of the unmanned vehicle in the three-dimensional environment space, and generate a set of candidate paths through polynomial fitting. The global planning module is used to extract the central axis based on the obstacle distance field to construct a pipe-shaped traversable corridor, and to select a global reference path that meets the kinematic constraints from the candidate path set within the traversable corridor. The index generation module is used to generate an index path based on the current position and the target position on the global path, guided by the global reference path. The trajectory planning module is used to select sampling points around the index path according to a set step size and width, construct an undirected graph by connecting the sampling points, remove edges and vertices that cross obstacles in the undirected graph, and query the undirected graph to obtain the collision-free local execution trajectory from the current position to the target position. The control execution module is used to solve for the control instruction sequence that minimizes the sum of state deviation, control deviation, and control change amplitude within the prediction period, with reference to the collision-free local execution trajectory. The module then applies the first instruction in the control instruction sequence to the actuator to complete the navigation task of the current control cycle.

Images