Unmanned aerial vehicle track generation method and system based on observability manifold

Abstract

The invention provides an unmanned aerial vehicle track generation method and system based on an observability manifold, and relates to the technical field of mobile robot autonomous navigation and intelligent control. The method comprises the following steps: acquiring and preprocessing sensor information; constructing a real-time observability manifold analysis model based on the preprocessed sensor information, and generating observability manifold constraints; constructing a viewpoint migration graph based on observability manifold constraint, and searching to generate a local guide target state; constructing a local localizable flight corridor based on a local guide target state, and generating a space-time corridor constraint; and based on space-time corridor constraints, constructing a space-time joint trajectory optimization model, performing joint optimization on the position trajectory and the yaw angle trajectory, solving an optimal control point, and generating a flight trajectory. According to the method, the problems of state estimation drift and navigation failure caused by observation constraint deficiency in a geometric degradation environment are solved, and the navigation robustness and efficiency are effectively considered on the premise of ensuring the positioning precision.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile robot autonomous navigation and intelligent control, and in particular to a UAV trajectory generation method and system based on observability manifold. BACKGROUND

[0002] With the rapid development of UAV technology, its application has extended from open outdoor to complex indoor and underground environments such as tunnels and mines. In these GPS failure scenarios, UAVs mainly rely on their own onboard sensors such as lidar, cameras, and IMUs to achieve positioning and mapping.

[0003] Currently, lidar SLAM (simultaneous localization and mapping) algorithms have high accuracy in environments with rich features, but in geometrically degenerate environments (such as long straight tunnels and smooth corridors), the point cloud lacks geometric constraints in certain directions, making state estimation unobservable and causing positioning drift and even system divergence, i.e., the "degeneration" phenomenon.

[0004] Existing technologies mainly fall into two categories: passive degeneration processing and active perception planning. The former takes remedial action after detecting degeneration, which often fails to reverse the accumulated errors. The latter considers feature visibility when planning the path, but relies on prior known maps or simple feature counting, and fails to assess the geometric contribution of features in spatial distribution, making it difficult to balance positioning robustness and exploration efficiency in unknown environments.

[0005] Therefore, there is an urgent need for a method that can predict degeneration risk in real time and solve the problem of navigation failure caused by inaccurate positioning in feature-poor environments, enabling safe and efficient autonomous flight in unknown degenerate scenarios. SUMMARY

[0006] The present application aims to provide a UAV trajectory generation method and system based on observability manifold, which solves the problem of state estimation drift and navigation failure caused by the lack of effective observation constraints in geometrically degenerate environments, enabling the UAV to complete obstacle avoidance and exploration tasks while autonomously adjusting its flight attitude in extreme environments, effectively balancing the robustness and efficiency of navigation while ensuring positioning accuracy.

[0007] To achieve the above-mentioned purpose, the present application provides a UAV trajectory generation method based on observability manifold, comprising the following steps: Step S1, acquiring sensor information and preprocessing; Step S2, based on the preprocessed sensor information, constructing a real-time observability manifold analysis model to generate an observability manifold constraint; Step S3, constructing a view migration graph based on the observability manifold constraint, searching for a local guide target state; Step S4, constructing a local locatable flight corridor based on the local guide target state, generating a space-time corridor constraint; Step S5, constructing a space-time joint trajectory optimization model based on the space-time corridor constraint, jointly optimizing the position trajectory and the yaw angle trajectory, solving the optimal control point, and generating the flight trajectory.

[0008] Preferably, step S2 specifically comprises: Based on the correspondence between the point pairs in the laser radar point cloud and the planes in the local map, an alignment error model is constructed, and the alignment error is calculated for the i-th point pair in the point cloud , the specific expression is: ; ; Wherein, represents the total number of point pairs; represents the rotation matrix, ; represents the translation vector, ; represents the current frame point; represents the point on the corresponding plane in the local map; represents the normal vector of the corresponding plane in the local map; represents the three-dimensional rotation group; represents the three-dimensional Euclidean space; The linearization of the alignment error is performed using the Lie algebra perturbation model, and the specific expression is: ; ; Wherein, is the residual term; represents the state increment; represents the Jacobian matrix of each point pair; represents the transpose; represents the rotation increment; represents the translation increment; The system Hessian matrix is constructed, and eigenvalue decomposition is performed to obtain the distribution of the eigenvalue spectrum ; According to the distribution of the eigenvalue spectrum , the degenerate direction is determined, and the observability covariance matrix is generated; The ellipsoid space defined by the observability covariance matrix is the observability manifold constraint, and the specific expression is: ;​ ; ; ; wherein, represents a feature vector matrix.

[0009] Preferably, step S2 further comprises: generating an observability confidence based on the observability covariance matrix, and the specific expression is: ; ; ; wherein, represents a translation confidence; represents a rotation confidence; represents the number of constraints whose projection component on a certain degree of freedom is greater than a threshold value; represents the total number of constraints; represents a set of observable degrees of freedom; represents a diagonal matrix constructor; , , represents the number of constraints whose projection component on the x-axis, y-axis, z-axis translation degree of freedom of the UAV is greater than a threshold value, respectively; , , represents the number of constraints whose projection component on the roll, pitch, yaw rotation degree of freedom of the UAV is greater than a threshold value, respectively; When the observability confidence of a certain degree of freedom is lower than a preset safety threshold , it is determined that there is a high risk of degradation in the direction of this degree of freedom, and a strong perception constraint mode of trajectory generation is triggered.

[0010] Preferably, step S3 specifically comprises: Based on the grid map, a front view point is generated by sampling on the boundary of the unknown area, and a feature view point is generated by sampling on the overlapping visible area of the known feature cluster, and the front view point and the feature view point together constitute a view point set; wherein the front view point , the feature view point satisfy the following conditions: ; ; wherein, represents an indicator function, represents a field of view angle function, represents a front cover threshold value; represents a set of features visible from the viewpoint, represents a co-visible feature number threshold value; represents a front feature point; represents a current viewpoint of the UAV; For the viewpoints in the viewpoint set, all viewpoints achieving straight-line collision-free flight are determined and connected within the free space defined by the grid map, forming an edge set; Based on the observability manifold constraint, the edge cost of each edge in the edge set is calculated, and the specific expression is: ; wherein, represents the edge cost of an edge connecting the viewpoint to the viewpoint ; and are the maximum linear velocity and the maximum yaw rate, respectively; represents a unit directional vector from the viewpoint to the viewpoint ; represents a predicted observability covariance matrix of the path midpoint; represents a manifold constraint weight coefficient; represents a motion time cost; represents an observability manifold cost; represents a position vector of the viewpoint ; represents a position vector of the viewpoint ; Based on the current pose of the UAV, a current state viewpoint is determined, and the current state viewpoint is taken as a search starting point. Through the Dijkstra algorithm, a minimum cumulative cost path from the current state viewpoint to all front viewpoints is searched in the viewpoint transition graph, and the first front viewpoint in the minimum cumulative cost path is selected as a local guidance target state.

[0011] Preferably, step S4 specifically comprises: Taking the position of the current state viewpoint as a starting point and the position of the local guidance target state as an ending point, a collision-free polyline path is planned in the free space defined by the grid map using the Dijkstra algorithm, and is recorded as an initial path; Taking the initial path as the center, a series of mutually connected convex polyhedrons are generated using a map-based inflation algorithm based on the grid map; Key geometric features around the initial path are extracted , and then the initial path is sampled, and the angle boundary pair corresponding to each sampling point is calculated through the incremental bidirectional search method such that within the yaw angle range, the key geometric features contained in the UAV field of view corresponding observable matrix minimum eigenvalue greater than the preset robust threshold ; finally, the angle boundary pairs corresponding to all sampling points jointly constitute the perception angle constraints acting on the entire initial path; angle boundary pair and the convex polyhedron jointly construct a locally locatable flight corridor, generate a space-time corridor constraint, and the specific expression is: ; wherein, represents the number of constraints in the constraint set.

[0012] Preferably, in step S5, the space-time joint trajectory optimization model uses uniform B-spline curves to parameterize the position trajectory and the yaw angle trajectory , and each uniform B-spline curve is uniquely determined by a set of control points: position trajectory is defined by a set of position control points ; yaw angle trajectory is defined by a set of yaw angle control points ; construct an optimization objective function , and convert the observable manifold constraint into an observable manifold constraint penalty term, and the specific expression is: ; ; wherein, represents the trajectory smoothness penalty term; represents the obstacle avoidance penalty term based on the ESDF map; represents the feasibility penalty term of velocity and acceleration; represents the observable manifold constraint penalty term; represents the set of geometric features visible in the field of view at the current time; represents the Jacobian matrix corresponding to the th feature; represents the regularization parameter; represents the motion constraint weight; represents the smoothness weight coefficient; represents the collision weight coefficient; represents the dynamic weight coefficient; represents the determinant operation of the matrix; represents the total length of the trajectory in time.

[0013] Preferably, in step S5, the yaw angle trajectory in the optimization objective function is optimized in the unconstrained space by using differential flatness transformation. The solution is obtained by using the following specific expression: wherein, and and represent the continuous-time function obtained by linear interpolation or B-spline fitting on the discrete angle boundary pair . represents the normalized unconstrained mapping variable of the yaw angle.

[0014] The application also provides a UAV trajectory generation system based on an observability manifold, which adopts the above method and comprises a perception front-end module, a degeneration risk assessment module, a local target decision module and a trajectory planning and control module. The perception front-end module is used for receiving and pre-processing sensor data, performing feature extraction and point cloud registration, and synchronously outputting the current pose of the UAV. The degeneration risk assessment module is used for receiving the pre-processed sensor information, analyzing the point cloud registration process, constructing a real-time observability manifold analysis model, evaluating the observability confidence of the current pose, and outputting the observability manifold constraint. The local target decision module is used for constructing a viewpoint migration graph according to the observability manifold constraint, and searching for a local guide target state according to the viewpoint migration graph. The trajectory planning and control module is used for constructing a local locatable flight corridor and generating a space-time corridor constraint according to the local guide target state, constructing a space-time joint trajectory optimization model based on the space-time corridor constraint, jointly optimizing the position trajectory and the yaw angle trajectory, generating a flight trajectory, and outputting a control instruction.

[0015] Preferably, the degeneration risk assessment module comprises a dynamic weight adjuster, which sends a signal to the trajectory planning and control module when detecting that the observability confidence of a certain degree of freedom is lower than a preset safety threshold, so that the trajectory planning and control module increases the motion constraint weight of the degeneration direction when constructing the optimization objective function, or forcibly adjusts the yaw angle boundary when constructing the local locatable flight corridor, and introduces the observation of geometric features orthogonal to the degeneration direction.

[0016] Therefore, the UAV trajectory generation method and system based on the observability manifold have the following beneficial technical effects: ​​​(1) The present application changes from "passive remedy" to "active early warning", and improves environmental adaptability. By constructing a real-time observable manifold analysis model, and using the distribution of the eigenvalue spectrum of the system Hessian matrix to accurately quantify the alignment risk of point cloud registration in six degrees of freedom space, the unmanned aerial vehicle can predict in advance whether there is a positioning degradation risk in the front area (such as the smooth tunnel axis direction), so as to actively avoid at the planning level, and fundamentally eliminate the drift divergence.

[0017] (2) The present application solves the contradiction between "exploration" and "positioning", and realizes the win-win of navigation efficiency and robustness. Through the viewpoint migration graph, the observability manifold constraint is innovatively introduced into the edge cost calculation, forcing the unmanned aerial vehicle to adaptively select the path that can both promote the task and provide rich geometric constraints (for example, walking "zigzag" in the corridor or flying close to the side with rich texture) in the exploration process, effectively solving the wandering pain point in the feature-poor environment.

[0018] (3) The present application realizes strong coupling control of "pose-sensing", and ensures flight safety in extreme environment. Through the local positionable flight corridor and the space-time joint trajectory optimization model, and introducing Fisher information maximization as a penalty term, the yaw angle is strictly constrained by using differential flatness transformation, forcing the unmanned aerial vehicle to always lock the field of view on the key geometric features while avoiding obstacles, and improving the survival ability in narrow and textureless space. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 The flowchart of the unmanned aerial vehicle trajectory generation method based on the observability manifold of the present application; Figure 2 The structure schematic diagram of the unmanned aerial vehicle trajectory generation system based on the observability manifold of the present application. DETAILED DESCRIPTION

[0020] The technical solutions of the present application are further described below through the drawings and examples.

[0021] Unless otherwise defined, the technical terms or scientific terms used in the present application shall have the usual meanings understood by those skilled in the art to which the present application belongs.

[0022] Example 1 As shown in Figure 1 A kind of unmanned aerial vehicle trajectory generation method based on the observability manifold, comprising the following steps: Step S1, obtaining sensor information and preprocessing.

[0023] Sensor information includes environment information and self-state information collected by unmanned aerial vehicle on-board sensors (such as laser radar and IMU), environment information includes laser radar point cloud data and IMU data (including angular velocity and acceleration), self-state information including current pose (including position , rotation and IMU bias ) and velocity .

[0024] The preprocessing process specifically includes: firstly, hard trigger or soft timestamp synchronization is implemented on the lidar point cloud data and IMU data; then, the motion distortion in the single frame scanning of the lidar is removed by using the high-frequency angular velocity and acceleration integral data of the IMU to project all points to the coordinate system at the starting time of the frame; subsequently, the original point cloud is down-sampled by using a voxel filter to remove redundant data and reduce the calculation load; finally, the corrected point cloud is used to update the occupancy grid map based on the ray casting method, thereby providing an environmental basis for subsequent viewpoint visibility checking and obstacle avoidance planning.

[0025] In step S2, based on the preprocessed sensor information, a real-time observability manifold analysis model is constructed to generate an observability manifold constraint. Specifically, the following steps are included: The lidar point cloud data and the local map are registered to establish a correspondence between the point pairs in the lidar point cloud and the planes in the local map.

[0026] In order to evaluate the geometric degradation degree of the current environment, based on the correspondence between the point pairs in the lidar point cloud and the planes in the local map, an alignment error model is constructed, and for the i-th point pair in the point cloud , the alignment error is calculated, and the specific expression is as follows: ; wherein, represents the total number of point pairs; represents a rotation matrix, ; represents a translation vector, ; represents the current frame point; represents the point on the corresponding plane in the local map; represents the normal vector of the corresponding plane in the local map; represents a three-dimensional rotation group; represents a three-dimensional Euclidean space.

[0027] The alignment error is linearized by using a Lie algebra perturbation model, and the specific expression is as follows: ; ; wherein, is a residual term;​ Indicates the state increment; This represents the Jacobian matrix for each pair of points; Indicates transpose; Indicates the rotation increment; This indicates the translation increment.

[0028] Constructing the Hessian matrix of the system Then, eigenvalue decomposition is performed to obtain the eigenvalue spectrum. The distribution; based on the eigenvalue spectrum The distribution of the data is used to determine the degradation direction and generate a six-degree-of-freedom observable covariance matrix in the current pose space. The ellipsoidal space defined by the observable covariance matrix is ​​the observable manifold constraint, used to characterize the degree of environmental geometric degradation. Its specific expression is: ; ; ; ; in, This represents the eigenvector matrix.

[0029] To intuitively quantify degradation risk, based on the observable covariance matrix Generate normalized observability confidence. The specific expression is: ; ; ; in, Indicates the confidence level of the translation; Indicates the rotation confidence level; Indicates a specific degree of freedom The number of constraints whose upper projection components are greater than the threshold; Indicates the total number of constraints; Represents the set of observable degrees of freedom; Describes the constructor for a diagonal matrix; , , These represent the number of constraints where the projected components are greater than the threshold in the translational degrees of freedom of the UAV along the x, y, and z axes, respectively. , , These represent the number of constraints where the projected components are greater than the threshold in the roll, pitch, and yaw rotation degrees of freedom of the UAV, respectively.

[0030] Observability confidence For observable covariance matrix (or its inverse matrix, i.e., the Hessian matrix of the system) The normalized representation of the characteristic distribution of the system is as follows: Specifically, the Hessian matrix of the system. This reflects the strength of the constraint, among which It contains the normal vector of the corresponding plane in the local map. The information. And the formula above. The terms actually represent the system's Hessian matrix. The effective constraint components of the main diagonal elements in the corresponding axis.

[0031] When the observability confidence of a certain degree of freedom is lower than a preset safety threshold (The value range is [0.1, 0.3], and it is generally set to 0.2). When this value is set to [0.1, 0.3], it is determined that there is a high risk of degradation in this degree of freedom direction, and the strong perception constraint mode for trajectory generation is triggered.

[0032] Step S3: Based on observable manifold constraints, construct a viewpoint migration map based on the environmental front and feature distribution, and search to generate a local guided target state including position and yaw angle. Specifically, this includes: Viewpoint shift graph is composed of a set of viewpoints Sum of edges Composition, each viewpoint in the viewpoint shift diagram Defined as a state tuple, the specific expression is: ; in, Indicates location; Indicates the yaw angle.

[0033] Based on a raster map, clustering is performed at the boundaries of unknown regions, and each front cluster is identified. Nearby front viewpoints are generated by sampling in cylindrical coordinates. In the known feature cluster and Overlapping visible areas Intrasampling generates feature viewpoints Frontier Perspectives and feature viewpoint Together they constitute a set of viewpoints; among them, the frontier viewpoints Feature viewpoint The following conditions must be met: ; ; in, Indicates an indicator function, Represents the field of view angle function. Indicates the frontal coverage threshold; This represents the set of features visible from a viewpoint. This represents the threshold for the number of shared features; Represents leading-edge feature points; This represents the current viewpoint of the drone. For each viewpoint in the viewpoint set, within the free space defined by the grid map, identify and connect all viewpoints that achieve straight-line collision-free flight, forming an edge set.

[0034] To proactively avoid degenerate regions during path search, an edge cost is calculated for each edge in the edge set based on observable manifold constraints. This edge cost combines the motion time cost with the observable manifold cost, and its specific expression is as follows: ; in, Indicates the connection viewpoint To viewpoint The edge cost of one edge; and These are the maximum linear velocity and the maximum yaw rate, respectively. Indicates from the viewpoint To viewpoint The unit direction vector; The predictive observability covariance matrix of the midpoint of the path; Indicates the manifold constraint weight coefficient; Indicates the time cost of exercise; Represents the cost of observable manifolds; Indicate viewpoint The position vector; Indicate viewpoint The position vector. The above formula means that if the direction of movement is consistent with the direction of high uncertainty in the environment (the direction of degradation), the edge cost will increase exponentially.

[0035] Based on the current pose of the UAV, the current state viewpoint is determined. Using the current state viewpoint as the search starting point, the Dijkstra algorithm is used to search for the minimum cumulative cost path from the current state viewpoint to all leading viewpoints in the viewpoint migration graph. The first leading viewpoint in the minimum cumulative cost path is selected as the local guided target state.

[0036] Among them, let the path , Representing the viewpoint, the cumulative cost of that path is... Defined as the sum of the costs of all adjacent viewpoint edges along the path, specifically expressed as: ; in, Indicates the connection viewpoint To viewpoint the edge cost of one edge of the path.

[0037] Step S4, based on the local guidance target state, constructing a local locatable flight corridor, generating a space-time corridor constraint. Specifically including: Taking the position of the current state viewpoint as the starting point, and the position of the local guidance target state as the ending point, a collision-free polyline path is planned in the free space defined by the grid map using Dijkstra algorithm, denoted as the initial path.

[0038] Taking the initial path as the center, based on the grid map, using the map-based inflation algorithm (such as the Douglas-Pok algorithm) to generate a series of mutually connected convex polyhedrons .

[0039] Extracting key geometric features around the initial path Then sampling the initial path, and calculating the corresponding angle boundary pair of each sampling point through the incremental bidirectional search method So that within the yaw angle range, the key geometric features contained in the field of view of the UAV Make the minimum eigenvalue of the corresponding observability matrix greater than the preset robustness threshold (the value range is [10, 50]), the specific process is: Taking the tangent angle of the current sampling point as the center, incremental search is performed to the left and right sides. For each test angle , detect whether the feature point set in the field of view satisfies , and the maximum continuous angle range that meets the condition is .

[0040] Finally, the angle boundary pairs corresponding to all sampling points jointly constitute the perception angle constraint acting on the entire initial path.

[0041] The angle boundary pair and the convex polyhedron jointly construct a local locatable flight corridor to generate a space-time corridor constraint , the specific expression is: ; Wherein, represents the number of constraints in the constraint set.

[0042] The local locatable flight corridor is used to constrain the spatial position range and yaw angle coverage range of the trajectory generation, ensuring that the trajectory meets the perception constraint while avoiding obstacles.

[0043] Step S5, based on the space-time corridor constraint, a space-time joint trajectory optimization model is constructed to jointly optimize the position trajectory and the yaw angle trajectory, solve the optimal control point, and generate the flight trajectory.

[0044] The space-time joint trajectory optimization model adopts uniform B-spline curves to parameterize the position trajectory and the yaw angle trajectory . The position trajectory is defined by order spline basis functions and control points , and the specific expression is: ; Similarly, the yaw angle trajectory is defined by control points . Each uniform B-spline curve is uniquely determined by a set of control points: The position trajectory is defined by a set of position control points .

[0045] The yaw angle trajectory is defined by a set of yaw angle control points .

[0046] An optimization objective function is constructed, and the observability manifold constraint is converted into an observability manifold constraint penalty term, and the specific expression is: ; ; wherein represents the trajectory smoothness penalty term; represents the obstacle avoidance penalty term based on the ESDF map; represents the speed and acceleration feasibility penalty term; represents the observability manifold constraint penalty term; represents the set of geometric features visible within the current time field of view; represents the Jacobian matrix corresponding to the th feature; represents the regularization parameter; represents the motion constraint weight; represents the smoothness weight coefficient, used to adjust the importance of the trajectory smoothness; represents the collision weight coefficient, used to adjust the priority of the obstacle avoidance constraint; represents the dynamic weight coefficient, used to adjust the weight of the unmanned aerial vehicle dynamics constraint; represents the determinant operation of a matrix; represents the total length of the trajectory in time.

[0047] The observability manifold constraint penalty term aims to maximize the Fisher information along the trajectory integral, thus minimizing the uncertainty of the pose estimate.

[0048] To ensure that the yaw angle strictly satisfies the boundary pair , that is, to ensure that the UAV can certainly see the key geometric features , the optimization objective function is constructed as follows: The differential flatness transformation is adopted, and the yaw angle trajectory is not directly optimized, but the unconstrained space is optimized, and the specific expression is as follows: ; ; wherein and represent the continuous time functions obtained by linear interpolation or B-spline fitting on the discrete angle boundary pair ; represents the normalized unconstrained mapping variable of the yaw angle.

[0049] The position trajectory and the yaw angle trajectory are jointly optimized, that is, the optimization objective function is iteratively solved by a nonlinear optimization solver (such as NLopt or IPOPT), to obtain the optimal control points and , and then the continuous time parameterized trajectories and are generated according to the B-spline formula, as the final generated flight trajectory.

[0050] As shown in Figure 2 , a UAV trajectory generation system based on an observability manifold adopts the above method, and includes a perception front-end module, a degeneration risk assessment module, a local target decision module, and a trajectory planning and control module.

[0051] The perception front-end module is used for receiving and preprocessing sensor data, collecting environmental data in real time through a UAV on-board sensor (such as a laser radar and an IMU), and environmental information includes laser radar point cloud data and inertial measurement unit data (including angular velocity and acceleration). Feature extraction and point cloud registration are performed. At the same time, an error state Kalman filter (ES-EKF) based odometer front-end is used to output the current state information of the UAV (including position , rotation , velocity , and IMU zero offset ) in real time.

[0052] The degeneration risk assessment module is configured to receive the preprocessed sensor information, analyze the point cloud registration process, construct a real-time observability manifold analysis model, assess the observability confidence of the current pose, and output the observability manifold constraint. The degeneration risk assessment module aims to quantify the constraint capability of the current environment geometry on state estimation. The degeneration risk assessment module includes a dynamic weight adjuster. When the observability confidence of a certain degree of freedom (e.g., X-axis translation) is detected to be lower than a preset safety threshold (the value range is [0.1, 0.3], and the value is 0.2 in the embodiment), the dynamic weight adjuster sends a signal to the trajectory planning and control module, so that the trajectory planning and control module increases the motion constraint weight of the corresponding degeneration direction when constructing the optimization objective function , or forcibly adjusts the yaw angle boundary when constructing the locally locatable flight corridor, forcing the UAV to make a large attitude adjustment (e.g., side flight), introducing geometric feature observation orthogonal to the degeneration direction, thereby eliminating the drift risk of state estimation.

[0053] The local target decision module is configured to construct a viewpoint migration graph according to the observability manifold constraint, and search for a local guide target state containing position and yaw angle according to the viewpoint migration graph.

[0054] The trajectory planning and control module is configured to construct a locally locatable flight corridor and generate a space-time corridor constraint according to the local guide target state; construct a space-time joint trajectory optimization model based on the space-time corridor constraint, jointly optimize the position trajectory and the yaw angle trajectory, generate a flight trajectory, and output a control instruction.

[0055] Therefore, the unmanned aerial vehicle trajectory generation method and system based on the observability manifold solve the problem of state estimation drift and navigation failure caused by the lack of effective observation constraint in the prior art in a geometric degeneration environment, and can enable the unmanned aerial vehicle to complete the obstacle avoidance and exploration task while autonomously and safely adjusting the flight attitude in an extreme environment, effectively balancing the robustness and efficiency of navigation while ensuring positioning accuracy.

[0056] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application but not to limit them, although the present application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or replaced by equivalents, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present application.

Claims

1. A method for generating a trajectory of an unmanned aerial vehicle (UAV) based on an observability manifold, the method comprising: The method comprises the following steps: Step S1, obtaining sensor information and preprocessing; Step S2, constructing a real-time observability manifold analysis model based on the preprocessed sensor information to generate an observability manifold constraint; Step S3, constructing a viewpoint migration graph based on the observability manifold constraint to search for a local guide target state; Step S4, constructing a locally locatable flight corridor based on the local guide target state to generate a space-time corridor constraint; Step S5, constructing a space-time joint trajectory optimization model based on the space-time corridor constraint to jointly optimize a position trajectory and a yaw angle trajectory, solving an optimal control point to generate a flight trajectory. 2.The method of claim 1, wherein, Step S2 specifically comprises: Based on the correspondence between the point pairs in the laser radar point cloud and the planes in the local map, an alignment error model is constructed, and for the point pairs in the point cloud , the alignment error is calculated, and the specific expression is: ​ ； wherein, denotes the total number of point pairs; denotes a rotation matrix, ; denotes a translation vector, ; denotes a point of the current frame; denotes a point on the corresponding plane in the local map; denotes a normal vector of the corresponding plane in the local map; denotes the three-dimensional rotation group; denotes the three-dimensional Euclidean space; Using the perturbation model of Lie algebra to align error is linearized, and the specific expression is ； ； wherein, is a residual term; denotes a state increment; denotes a Jacobian matrix for each point pair; denotes a transpose; denotes a rotation increment; denotes a translation increment; Constructing the system hessian matrix , and performing eigenvalue decomposition to obtain the distribution of eigenvalue spectrum ; according to the distribution of eigenvalue spectrum , determine the degenerate direction, and generate the observability covariance matrix ; the ellipsoid space defined by the observability covariance matrix is the observability manifold constraint, and the specific expression is: ； ； ； ； wherein, denotes the feature vector matrix. 3.The method of claim 2, wherein, Step S2 further comprises: generating an observability confidence according to the observability covariance matrix The specific expression is: ； ； ； wherein, represents a translational confidence; represents a rotational confidence; represents the number of constraints with projected component greater than a threshold on a particular degree of freedom represents the total number of constraints; represents the set of observable degrees of freedom; represents a diagonal matrix constructor; , , represents the number of constraints with projected component greater than a threshold on the x, y, z translational degrees of freedom of the drone respectively; , , represents the number of constraints with projected component greater than a threshold on the roll, pitch, yaw rotational degrees of freedom of the drone respectively;​ When the observability confidence of a certain degree of freedom is lower than a preset safety threshold a high-risk degradation exists in the direction of this degree of freedom, and a strong perception constraint mode of trajectory generation is triggered.

4. The method of claim 3, wherein, Step S3 specifically comprises: Based on the grid map, a front view point is generated by sampling at the unknown region boundary, a feature view point is generated by sampling at the overlapping visible region of the known feature cluster, and the front view point and the feature view point together constitute a view point set; wherein the front view point , the feature view point satisfy the following conditions: ； ； wherein, represents an indicator function, represents a field of view angle function, represents a front cover threshold value; represents a feature set visible from a viewpoint, represents a common view feature quantity threshold value; represents a front feature point; represents a current viewpoint of the unmanned aerial vehicle; for a viewpoint in the viewpoint set, all viewpoints achieving straight-line collision-free flight are judged and connected within the free space defined by the grid map, forming an edge set; Based on the observability manifold constraint, the edge cost of each edge in the edge set is calculated, and the specific expression is: ； wherein, denotes the edge cost of a connecting view point to a view point ; and are the maximum linear and yaw angular velocities, respectively; denotes the unit direction vector from view point to view point ; denotes the predicted observability covariance matrix of the path mid-point; denotes the manifold constraint weight coefficient; denotes the motion time cost; denotes the observability manifold cost; denotes the position vector of view point ; denotes the position vector of view point ; Based on the current pose of the unmanned aerial vehicle, a current state viewpoint is determined, the current state viewpoint is taken as a search starting point, a minimum cumulative cost path from the current state viewpoint to all front view points is searched in the viewpoint migration graph through a Dijkstra algorithm, and the first front view point in the minimum cumulative cost path is selected as the local guide target state.

5. The method of claim 4, wherein, Step S4 specifically comprises: Taking the position of the current state viewpoint as a starting point and the position of the local guide target state as an ending point, a collision-free polyline path is planned in the free space defined by the grid map using the Dijkstra algorithm, and the path is recorded as an initial path. Based on the initial path, a series of mutually connected convex polyhedrons are generated using a map-based inflation algorithm for spatial constraint based on a grid map ; Extracting key geometric features around the initial path Then, the initial path is sampled, and the angle boundary pair corresponding to each sampling point is calculated by incremental bidirectional search method So that within the yaw angle Range, the key geometric features contained in the field of view of the UAV Make the minimum eigenvalue of the corresponding observability matrix Greater than the preset robust threshold Finally, the angle boundary pairs corresponding to all sampling points jointly constitute the perception angle constraint acting on the entire initial path angle boundary pair with convex polyhedron co-constructing a locally positionable flight corridor, generating a spatiotemporal corridor constraint, expressed as: ； wherein represents the number of constraints in the constraint set.

6. The method of claim 5, wherein, In step S5, the spatio-temporal joint trajectory optimization model parameterizes the position trajectory and the yaw angle trajectory using uniform B-spline curves, each of which is uniquely determined by a set of control points: Position trajectory From a set of position control points Definition; Yaw angle trajectory From the yaw angle control point set Definition; construct the optimization objective function And the observability manifold constraint is converted into the observability manifold constraint penalty term, and the specific expression is: ； ； in, This represents the trajectory smoothness penalty term; This indicates the obstacle avoidance penalty based on the ESDF map; Indicates the feasibility penalty for velocity and acceleration; This represents the observability manifold constraint penalty term; This represents the set of geometric features visible within the field of view at the current moment; Indicates the first The Jacobian matrix corresponding to each feature; Represents the regularization parameter; Indicates the weight of motion constraints; Indicates the weighting coefficients of the smoothing term; This represents the collision term weighting coefficient; Indicates the weight coefficient of the dynamic term; Represents the determinant operation of a matrix; This represents the total time length of the trajectory.

7. The method of claim 6, wherein, In step S5, for optimizing the objective function Yaw angle trajectory Differential flattening transformation is used to optimize the unconstrained space. The solution is as follows: ； ； wherein, and denotes a continuous time function resulting from linear interpolation or B-spline fitting of the discrete angular boundary pairs denotes a normalized unconstrained mapping variable of the yaw angle.​ 8. An observable manifold based UAV trajectory generation system employing the method of any one of claims 1-7. The system comprises a perception front-end module, a degradation risk assessment module, a local target decision module, and a trajectory planning and control module. The perception front-end module is used for receiving and preprocessing sensor data, performing feature extraction and point cloud registration, and synchronously outputting a current pose of the unmanned aerial vehicle. The degradation risk assessment module is used for receiving preprocessed sensor information, analyzing a point cloud registration process, constructing a real-time observability manifold analysis model, evaluating an observability confidence of the current pose, and outputting an observability manifold constraint. The local target decision module is used for constructing a viewpoint migration graph according to the observability manifold constraint, and searching for a local guide target state according to the viewpoint migration graph. The trajectory planning and control module is used for constructing a locally locatable flight corridor according to the local guide target state and generating a space-time corridor constraint; constructing a space-time joint trajectory optimization model based on the space-time corridor constraint, jointly optimizing a position trajectory and a yaw angle trajectory, generating a flight trajectory, and outputting a control instruction.

9. The observable manifold based UAV trajectory generation system of claim 8, wherein, The degradation risk assessment module comprises a dynamic weight adjuster. When it is detected that the observability confidence of a certain degree of freedom is lower than a preset safety threshold, the dynamic weight adjuster sends a signal to the trajectory planning and control module, so that the trajectory planning and control module increases the motion constraint weight of the degradation direction in constructing an optimization objective function, or forcibly adjusts the yaw angle boundary in constructing the locally locatable flight corridor, and introduces a geometric feature observation orthogonal to the degradation direction.

Images