Search

CN-122017917-A - GNSSINS continuous time track optimization method based on self-adaptive control point

CN122017917ACN 122017917 ACN122017917 ACN 122017917ACN-122017917-A

Abstract

The invention relates to the technical field of satellite navigation and inertial navigation combined positioning and discloses a GNSSINS continuous time track optimization method based on self-adaptive control points, which comprises the following steps of S1, track segmentation and uncertainty analysis, acquisition of discrete moment state estimation and covariance matrix of a GNSS/INS combined navigation filter, identification of GNSS update moment and track segmentation, S2, distribution of geometrically-driven control points, calculation of discrete curvature of axial smooth position data, identification and refinement of local curvature maximum value points, selection of global dominant control points, and generation of a 'pre-convergence' effect, wherein the method utilizes the local support characteristic of a B spline curve, enables GNSS recovery information to backtrack track estimation during interruption by an observation constraint mechanism, reduces 52% of position RMSE under a 30 second interruption scene, reduces 58% of peak error, reduces 66% of position RMSE under a 90 second interruption scene, and reduces 70% of peak error.

Inventors

  • SHI CHUANG
  • GU BO
  • LI TUAN

Assignees

  • 北京航空航天大学

Dates

Publication Date
20260512
Application Date
20260210

Claims (10)

  1. 1. The GNSSINS continuous time track optimization method based on the self-adaptive control point is characterized by comprising the following steps of: S1, track segmentation and uncertainty analysis, namely acquiring a discrete moment state estimation and covariance matrix of a GNSS/INS integrated navigation filter, identifying GNSS update moment and dividing track segments, extracting position and attitude standard deviation, defining accumulated uncertainty and dividing each track segment into three subsections bearing similar accumulated uncertainty; S2, distributing control points driven by geometry, calculating discrete curvature of each axial smooth position data, identifying and refining local curvature maximum value points, selecting global leading control points, optimizing encryption control points of a first subarea section through shape indexes, and increasing the distribution of sparse control points along with the serial numbers of the subareas section; s3, refining control points of the observation constraint, adopting a weighting strategy inversely proportional to the position uncertainty, adopting a backward attenuation correction strategy for the gesture, and dynamically optimizing control point configuration by combining the direction consistency constraint and the residual error constraint; S4, constructing a self-adaptive node vector and generating a B spline track, constructing a non-uniform node vector based on a control point timestamp, calculating a B spline basis function, and respectively adopting a weighted B spline curve and an accumulated B spline quaternion curve to represent a position track and a gesture track; And S5, outputting continuous time tracks, namely realizing the retrospective influence of GNSS recovery information by utilizing the local support characteristic of the B spline, acquiring the speed and acceleration at any moment by analyzing and differentiating, and outputting the C2 continuous pose tracks.
  2. 2. The adaptive control point-based GNSSINS continuous-time trajectory optimization method according to claim 1, wherein in step S1, the state estimation includes a position estimation Gesture quaternion State covariance matrix From covariance matrix Extract position sub-block Sum gesture sub-block Taking the square root of the diagonal element gives the standard deviation of each axis: Position standard deviation: attitude standard deviation: ; the attitude uncertainty is characterized by the norm of the triaxial standard deviation, expressed as follows: 。
  3. 3. The adaptive control point-based GNSSINS continuous-time trajectory optimization method according to claim 2, further comprising defining an accumulated uncertainty, based on a single-axis standard deviation accumulation for position and on pose for pose in step S1 Cumulatively, dividing each track segment into three subsections based on cumulative uncertainty 、 、 Causing each subsection to bear a similar cumulative uncertainty.
  4. 4. The adaptive control point-based GNSSINS continuous-time trajectory optimization method according to claim 3, wherein in step S2, for each axial direction Calculating a discrete curvature from the smoothed position data of (a) by: Wherein, the And The first and second derivatives of the smoothed position data, respectively.
  5. 5. The method for optimizing GNSSINS continuous time tracks based on adaptive control points according to claim 4, wherein in step S2, local curvature maximum points are identified, and the local curvature maximum points satisfy And is also provided with Setting a curvature threshold Filtering noise-induced false extrema, where Is the first The subsections being in the axial direction The average curvature over is refined LCM collection 。
  6. 6. The method for optimizing GNSSINS continuous time tracks based on adaptive control points according to claim 5, wherein in step S2, the segment boundary points are used as anchor points, and the point with the largest curvature is selected from the LCM set of each subsection as a representative control point, so as to form a dominant control point set: Wherein the method comprises the steps of And Respectively the first Starting and ending global index of segments, wherein Is the first Local index of the point of maximum curvature in the subsection, For mapping of local to global indexes, these five points constitute a sparse skeleton of the B-spline track.
  7. 7. The adaptive control point-based GNSSINS continuous-time trajectory optimization method according to claim 6, wherein in step S2, the first subsection Constructing an initial control point set comprising dominant control points, sub-segment endpoints and LCM points exceeding a threshold, minimizing geometric distortion by iterative optimization by performing B-spline fitting based on the current control point set, identifying the maximum residual point within the sub-segment, locating the control point interval containing the maximum residual point Calculating the shape index of the candidate partition point in the interval, wherein the expression is as follows: Wherein the method comprises the steps of 、 Is the integrated curvature of the two sub-intervals, 、 In order to correspond to the arc length, And Is interval of Total integrated curvature and total arc length of (a), weight coefficient ; A point that minimizes the difference in shape index between the two subintervals is selected as a new control point: Gradually sparsifying control point distribution along with the increase of the sequence number of the subarea, at The segment retains only a small number of high quality LCM points, The section only retains boundary points.
  8. 8. The method for optimizing GNSSINS continuous time tracks based on adaptive control points according to claim 1, wherein in the step S3, for the position, a weighted B-spline curve is used, and the weight is inversely proportional to the uncertainty: Wherein the method comprises the steps of Automatically converging the track to a high-reliability filtering update point in a high-uncertainty area; The direction consistency constraint is that the consistency of the connecting line direction of adjacent control points and the filtering estimation track direction is calculated, when the direction deviation exceeds a threshold value, control points are dynamically added in the interval, and the track direction is ensured to be consistent with the filtering estimation; The residual constraint is to dynamically increase control points in the region where the residual exceeds a threshold value by calculating a fitting residual between the B-spline curve and the filter estimation, so as to prevent abnormal fitting in the region with high uncertainty.
  9. 9. The adaptive control point-based GNSSINS continuous-time trajectory optimization method according to claim 1, wherein in step S4, a non-uniform node vector is constructed from the selected control point time stamps Setting the repeatability at two ends of the node vector as Wherein The number of B spline is% ); B spline basis function calculation based on Cox-de Boor recurrence formula ; For the position trace, a weighted B-spline curve representation is used: Wherein the method comprises the steps of In order to control the coordinates of the points, As a weight of the uncertainty, Is a B-spline basis function.
  10. 10. The method for optimizing GNSSINS continuous time trajectories based on adaptive control points according to claim 9, wherein in step S4, for the gesture trajectory, an accumulated B-spline quaternion curve is used: Wherein the method comprises the steps of For the attitude quaternion at the start of the segment, the incremental rotation vector is defined as: In order to accumulate the basis functions, Representing a quaternion multiplication, the representation ensuring a quaternion manifold Smooth evolution on the surface.

Description

GNSSINS continuous time track optimization method based on self-adaptive control point Technical Field The invention relates to the technical field of satellite navigation and inertial navigation combined positioning, in particular to a GNSSINS continuous time track optimization method based on a self-adaptive control point. Background The integrated navigation of a global satellite navigation system (GNSS) and an Inertial Navigation System (INS) is a real-time navigation scheme widely deployed in robots and automatic driving systems, wherein a filtering method based on Extended Kalman Filtering (EKF) is widely adopted due to high calculation efficiency and good real-time performance, however, when a vehicle runs through a region with serious GNSS signal shielding such as a tunnel, an urban canyon and the like, the filter can only rely on an inertial sensor to carry out dead reckoning, and prediction errors can be accumulated rapidly along with time. Among them, the prior art mainly has the following problems: (1) The traditional EKF method can carry out error correction after GNSS signals are recovered, trace backtracking optimization cannot be carried out on the track during interruption, and therefore positioning accuracy during interruption is difficult to guarantee; (2) The method for distributing the B spline control points by adopting uniform time intervals cannot adapt to geometric complexity and uncertainty change of different track sections, so that the distribution of computing resources is unreasonable; (3) The traditional method generates state estimation at discrete time, can not directly provide continuous state information such as position, speed and the like at any time, and is difficult to meet the requirements of downstream applications such as track planning, motion compensation and the like. Disclosure of Invention The invention provides a GNSSINS continuous time track optimization method based on a self-adaptive control point, which solves the technical problems in the related art. The invention provides a GNSSINS continuous time track optimization method based on an adaptive control point, which comprises the following steps: S1, track segmentation and uncertainty analysis, namely acquiring a discrete moment state estimation and covariance matrix of a GNSS/INS integrated navigation filter, identifying GNSS update moment and dividing track segments, extracting position and attitude standard deviation, defining accumulated uncertainty and dividing each track segment into three subsections bearing similar accumulated uncertainty; S2, distributing control points driven by geometry, calculating discrete curvature of each axial smooth position data, identifying and refining local curvature maximum value points, selecting global leading control points, optimizing encryption control points of a first subarea section through shape indexes, and increasing the distribution of sparse control points along with the serial numbers of the subareas section; s3, refining control points of the observation constraint, adopting a weighting strategy inversely proportional to the position uncertainty, adopting a backward attenuation correction strategy for the gesture, and dynamically optimizing control point configuration by combining the direction consistency constraint and the residual error constraint; S4, constructing a self-adaptive node vector and generating a B spline track, constructing a non-uniform node vector based on a control point timestamp, calculating a B spline basis function, and respectively adopting a weighted B spline curve and an accumulated B spline quaternion curve to represent a position track and a gesture track; And S5, outputting continuous time tracks, namely realizing the retrospective influence of GNSS recovery information by utilizing the local support characteristic of the B spline, acquiring the speed and acceleration at any moment by analyzing and differentiating, and outputting the C2 continuous pose tracks. In a preferred embodiment, in step S1, the state estimation comprises a position estimationGesture quaternionState covariance matrixFrom covariance matrixExtract position sub-blockSum gesture sub-blockTaking the square root of the diagonal element gives the standard deviation of each axis: Position standard deviation: attitude standard deviation: ; the attitude uncertainty is characterized by the norm of the triaxial standard deviation, expressed as follows: 。 In a preferred embodiment, step S1 further comprises defining an accumulated uncertainty based on a single axis standard deviation accumulation for position and on pose Cumulatively, dividing each track segment into three subsections based on cumulative uncertainty、、Causing each subsection to bear a similar cumulative uncertainty. In a preferred embodiment, in step S2, for each axial directionCalculating a discrete curvature from the smoothed position data of (a) by: Wherein, the AndThe first and second deriv