Autonomous positioning method for resisting bumping and environmental degradation without satellite navigation

Abstract

The invention discloses a self-positioning method for resisting bumping and environmental degradation without satellite navigation. The method comprises the following steps: firstly, extracting a bumping and distortion resistant robustness fusion feature element of a multi-source sensor; then, mismatching in feature matching is eliminated through semantic information constraint, and environmental vision and geometric feature degradation caused by severe weather and bumping vibration is overcome; meanwhile, adaptively judging whether the environmental characteristics are seriously degraded or not by analyzing the characteristic values of the observation matrix, and accurately positioning through pose estimation based on motion manifold in a scene in which the serious environmental characteristics are degraded and characteristic matching cannot be performed; when the environmental characteristics do not degrade seriously, the SLAM optimization framework based on the continuous time trajectory is adopted for accurate positioning. According to the method, the intelligent mobile robot or the unmanned vehicle can complete accurate self-positioning in a complex field environment without satellite navigation, bumping and vibration and environment degradation.

Description

Technical Field

[0001] This invention relates to the field of intelligent mobile robots or unmanned vehicles, and in particular to an autonomous positioning method that is resistant to bumps and environmental degradation without satellite navigation. Background Technology

[0002] In complex field environments, intelligent mobile robots or unmanned vehicles will be unable to obtain satellite navigation information when navigation satellite signals are blocked or interfered with. How to achieve accurate autonomous positioning in complex environments with no satellite navigation and simultaneously experiencing bumps, vibrations, and environmental degradation is a key technical problem that needs to be solved. Summary of the Invention

[0003] This disclosure provides an autonomous positioning method for resisting bumps and environmental degradation without navigation satellites. It enables intelligent mobile robots or unmanned vehicles to achieve accurate autonomous positioning in complex outdoor environments without satellite navigation and under conditions such as bumps, vibrations, and environmental degradation.

[0004] This method first extracts robust fusion feature primitives for resisting shock and distortion from multi-source sensors such as lidar, millimeter-wave radar, visible light camera, and infrared camera; Then, semantic information constraints are used to eliminate mismatches in feature matching and overcome the degradation of environmental visual and geometric features caused by severe weather and vibration. Meanwhile, by analyzing the eigenvalues ​​of the observation matrix to adaptively determine whether environmental features are severely degraded, in some wild hilly areas where severe environmental feature degradation makes feature matching impossible, accurate positioning will be achieved through pose estimation based on motion manifolds; when environmental features are not severely degraded, accurate positioning will be achieved using a SLAM optimization framework based on continuous time trajectories, ultimately completing autonomous positioning against turbulence and environmental degradation without navigation satellites.

[0005] Specifically, it mainly includes the following steps: S1, extract robust fusion feature primitives for anti-bump and anti-distortion properties of multi-source sensors; the multi-source sensors include multiple types such as lidar, visible light, millimeter-wave radar, and infrared cameras; S2 eliminates mismatches in feature matching by constraining semantic information, and overcomes the degradation of environmental visual and geometric features caused by severe weather and vibration. S3 adaptively judges whether environmental features are severely degraded by analyzing the eigenvalues ​​of the observation matrix; in scenarios where severe environmental feature degradation makes feature matching impossible, pose estimation is performed based on motion manifold constraints to resist environmental degradation; when environmental features are not severely degraded, an autonomous localization is performed using a SLAM optimization framework based on continuous time trajectories; finally, autonomous localization resistant to turbulence and environmental degradation is achieved without navigation satellites.

[0006] Furthermore, in step S1, the method for extracting robust feature primitives of the lidar includes: Point cloud data is corrected in real time using IMU inertial system information to obtain accurate pose information, and motion distortion is eliminated through rigid body pose changes. The specific steps include: ① The timestamp of each distance measurement is derived from the acquisition model based on lidar. ; ② Search for the missing data in the IMU's buffer queue. The two most recent left and right IMU data frames , It meets the following conditions: ; If not found, then search for two frames of data that meet the following conditions: .

[0007] ③Solve Interpolation is performed using quaternions:

[0008] ④ Solve the point cloud after distortion correction

[0009]

[0010] Based on this, the laser points in the current frame are all transformed to the lidar coordinate system corresponding to the start time of the frame by linear interpolation; After performing motion distortion culling on the laser point cloud, the curvature value c of each laser point is calculated based on the points to its left and right in the frame, and laser points parallel to the laser direction or those that are occluded are removed. .

[0011] Furthermore, in step S1, the method for extracting robust visible light visual feature primitives includes: ① Integrate IMU measurement information to eliminate rolling screen distortion in the original image caused by severe bumps, vibrations from mobile robots or unmanned vehicle platforms: Based on the timestamp of the exposure center of each frame image Exposure time difference with roller blind Calculate each scan line of each frame of the image, that is, the image's scan line. Line, corresponding timestamp as follows:

[0012] Based on the timestamps corresponding to each row of pixels and the velocity between adjacent frames estimated by the IMU, the pixels in each row of the image are compensated to the timestamps corresponding to the pixels in the first row of the image to remove the rolling screen distortion. ② Robust point, line, and surface feature extraction and inter-frame feature matching are performed, including: point feature extraction and matching using the ORB algorithm; line feature extraction using the improved LSD line extraction algorithm; and surface feature extraction using the PCA plane fitting algorithm.

[0013] Furthermore, in step S1, the method for extracting robust feature primitives of millimeter-wave radar includes: ① Correcting distortion caused by the Doppler effect: The distance to each target is corrected using the following formula:

[0014] in, , For transmission frequency, The radial component of the velocity; ②After Doppler correction, millimeter-wave radar feature extraction and inter-frame feature matching are performed.

[0015] Furthermore, in step S1, the method for extracting robust feature primitives of the infrared camera includes: using a neural network to perform corner detection and optical flow estimation on the infrared image.

[0016] Furthermore, the specific method of step S2 includes: (1) Use semantic segmentation to remove mismatches, including: The steps for removing 2D-2D mismatches are as follows: 1) Perform ORB feature extraction and matching on the 2D image; 2) Perform semantic segmentation on the matched image and remove matches with inconsistent categories; 3) Use the RANSAC algorithm again to further filter out mismatches, leaving the final matching pairs. To remove 2D-3D mismatches, the steps are as follows: 1) Perform feature extraction and matching on 2D images and 3D point clouds; 2) Perform semantic segmentation on the matched images and point clouds and remove matches with inconsistent categories; 3) Calculate the reprojection error using the PnP algorithm and filter out matching pairs with large errors. (2) Using semantic information to assist point cloud registration: Registration is performed separately based on the semantic labels of the features, and semantically relevant weights are added to the final optimization objective function; Differential downsampling of point clouds is performed based on their semantic categories; Semantics is used to constrain the plane fitting part in LOAM plane feature extraction; (3) Feature extraction: The LOAM feature extraction strategy is adopted, which specifically includes: Extract edge and planar features from the point cloud and calculate the roughness of each point:

[0017] in Indicates the target point. For other points on the same ring; then, use a threshold to classify these points into edge points and face points; each ring is divided into Each section; within each section, select... The sharpest point is used as the edge feature. The flattest point is used as the planar feature; finally, the edge feature points of the entire target frame are obtained. and planar feature points ; (4) Self-motion estimation of lidar: A semantically relevant weight is added to each loss function term. The overall optimization objective is:

[0018] in, It is the i-th edge feature point The distance to the line it corresponds to. It is the j-th planar feature point The distance to its corresponding plane, where l is the semantic category of the target feature point. These are semantically related weights; Here, let's assume a given edge feature point with semantic label l. Select an edge feature submap with the same semantic label l ,set up , If the target point is one of two distinct points on the line, then the distance from the target point to the line is calculated as follows: (8) set up , , Let there be three distinct points on the plane, then point The distance to the corresponding plane is: (9).

[0019] Furthermore, in step S3, the environmental degradation-resistant pose estimation based on motion manifold constraints specifically includes the following steps: (1) State prediction: State prediction is performed using measurements of odometer information, including prediction of pose and manifold parameters; Determine whether new image keyframes need to be inserted based on the predicted pose; (2) Status update: Add the pose of the latest keyframe to the state vector; Extracting and matching visual features from images; Construct each cost function term, including odometry information constraints, visual feature point constraints, and direct manifold constraints on pose; Perform bundle constraint adjustment based on iterative optimization to minimize the sum of cost function terms; Reparameterization of moving manifolds: Reparameterized moving manifolds based on the most recent pose estimation.

[0020] Furthermore, in step S3, the autonomous localization step based on the continuous-time trajectory SLAM optimization framework uses a continuous-time trajectory parameterized by B-splines to fuse multi-source observations from asynchronous, heterogeneous, and frequency-differentiated sensors, while simultaneously optimizing the continuous-time trajectory. Specifically, this includes: (1) Select 4th-order B-splines, of which cumulative B-splines are used; (2) Construction of constraint terms, including: 1) Inertial constraint term: The predicted values ​​of acceleration and angular velocity are obtained by differentiating the continuous time trajectory, and constraint terms are constructed with the observed values ​​of acceleration and angular velocity from the inertial navigation system. At the same time, a bias random walk constraint term is added. 2) Visual constraints: After extracting point features and line features, the constraints of RGB cameras and infrared cameras are the same. For point features, point reprojection constraints are constructed between the triangulated 3D points and the back projection of image pixels onto the 2D points on the normalized camera coordinate plane. For line features, a linear spatial line is constructed to perform coordinate transformation and reprojection constraints. At the same time, the spatial line is modeled using orthogonal representation during the optimization process. Laser constraints: For corner points, construct point-to-line constraints for the straight line formed by the current frame corner point and the local map corner point; for planar points, construct point-to-plane constraints for the plane formed by the current frame planar point and the local map planar point. Millimeter-wave constraint terms: Constraint terms are constructed based on the number of millimeter-wave scans and the target object; (3) Construct the overall objective function based on the constraints of each sensor, and use the Levenberg-Marquadt gradient descent algorithm to optimize the B-spline control points corresponding to the feature points of each sensor and the overall pose trajectory.

[0021] Compared with the prior art, the beneficial effects of this disclosure are: (1) enabling intelligent mobile robots or unmanned vehicles to complete accurate autonomous positioning in complex outdoor environments with no navigation satellites and with bumpy vibrations and environmental degradation; (2) improving the robustness and environmental adaptability of the positioning system of intelligent mobile robots or unmanned vehicles, and maintaining continuous and reliable positioning capabilities even in extreme scenarios with sparse visual features, drastic changes in illumination, or partial occlusion; (3) achieving efficient deep fusion of information from multiple heterogeneous sensors, providing a smoother and globally consistent positioning trajectory that surpasses any single sensor without significantly increasing the computational load; and (4) effectively improving the survivability of intelligent mobile robots or unmanned vehicles in complex outdoor environments. Attached Figure Description

[0022] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.

[0023] Figure 1 A schematic diagram of an autonomous positioning method that resists turbulence and environmental degradation in the absence of navigation satellites; Figure 2 This is a schematic diagram of the roller blind jelly effect; Figure 3 The timestamps are for each row of pixels, incrementing sequentially. Figure 4 Robust corner extraction from millimeter-wave radar images; Figure 5 Robust optical flow corner point extraction for infrared images; Figure 6 A visual illustration of semantic-based downsampling; Figure 7 A schematic diagram of robust feature matching with semantic constraints; Figure 8 This is a conceptual diagram of an autonomous vehicle on a moving manifold, with a global coordinate system. Mileage information coordinate system and camera coordinate system ; Figure 9 This is a diagram illustrating backend optimization. Detailed Implementation

[0024] Preferred embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure will be thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.

[0025] This disclosure provides an autonomous positioning method that is resistant to turbulence and environmental degradation in the absence of navigation satellites, mainly including the following steps: Extract robust fusion feature primitives for anti-bumping and anti-distortion of multi-source sensors such as lidar, visible light, millimeter-wave radar, and infrared camera; Eliminate mismatches in feature matching by using semantic information constraints, and overcome the degradation of environmental visual and geometric features caused by severe weather and bumpy vibrations. By analyzing the eigenvalues ​​of the observation matrix, an adaptive judgment is made on whether the environmental features are severely degraded. In scenarios where severe environmental feature degradation makes feature matching impossible, pose estimation is performed based on motion manifold constraints to resist environmental degradation. When the environmental features are not severely degraded, an autonomous localization is performed using a SLAM optimization framework based on continuous time trajectories. Finally, autonomous localization that is resistant to turbulence and environmental degradation is achieved without navigation satellites.

[0026] In one exemplary implementation, the specific process is as follows: Step 1: Robust feature primitive extraction and matching to resist turbulence and distortion To improve the platform's adaptability to severe weather, this embodiment uses infrared image features to ensure positioning accuracy in nighttime environments and introduces millimeter-wave radar features to achieve accurate positioning in rain, fog, and snow.

[0027] The first step in fusing multiple sensors is to extract robust features from sensor observations. In high-dynamic scenarios with large bumps, motion distortion will severely affect the raw measurement accuracy of the sensors. Inertial measurement units can accurately measure the angular velocity and acceleration of the platform's motion and can calibrate the raw measurement data from the sensors.

[0028] (1) Robust feature extraction of lidar: Complex ground environments such as undulating terrain and vibrations can cause distortion of lidar point cloud data on mobile robots or unmanned vehicles, which will seriously affect the construction of scale information and positioning accuracy. In this embodiment, the point cloud data is corrected in real time through IMU inertial system information to obtain its accurate pose information, and motion distortion is eliminated through rigid body pose changes. The main process includes: ① The timestamp of each distance measurement is derived from the acquisition model based on lidar. .

[0029] ② Search for the missing data in the IMU's buffer queue. The two most recent left and right IMU data frames , It meets the following conditions: .

[0030] If not found, then search for two frames of data that meet the following conditions: .

[0031] ③Solve Interpolation is performed using quaternions.

[0032] (1) ④ Solve the point cloud after distortion correction .

[0033] (2) Based on this, in order to extract robust laser features, the laser points of the current frame are all transformed to the lidar coordinate system corresponding to the start time of the frame by linear interpolation.

[0034] After performing motion distortion removal on the laser point cloud, the curvature value of each laser point is calculated based on the points to its left and right in the frame, and laser points that are parallel to the laser direction or are blocked are removed.

[0035] (3) (2) Visible light visual robust feature primitive extraction: Due to severe bumps, vibrations of intelligent mobile robots or unmanned vehicle platforms, motion images may be blurred, resulting in significant roller blind distortion (jelly effect, such as...). Figure 2 As shown in the figure, in order to extract robust visual features, it is necessary to fuse IMU measurement information to perform roller blind distortion removal on the original image.

[0036] Different timestamps on pixels cause rolling screen distortion when pixel rows from different timestamps are superimposed. This distortion can be corrected by using the timestamp of the exposure center of each frame. Exposure time difference with roller blind Calculate each scan line of each frame of the image (image frame number 1). The timestamp corresponding to the row) as follows: (4) Based on the timestamps corresponding to each row of pixels and the velocity between adjacent frames estimated by the IMU, the pixels in each row of the image are compensated to the timestamps corresponding to the pixels in the first row of the image, thereby removing the rolling screen distortion.

[0037] To achieve robust point, line, and area feature extraction and matching, the ORB (Oriented FAST and Rotated BRIEF) algorithm is used for point feature extraction and matching. ORB features exhibit good scale invariance, rotation invariance, and noise resistance. An improved LSD (Line Segment Detector) algorithm is employed to extract line features. Gaussian sampling is used to scale the input image, thereby reducing or even eliminating jagged edges. The NFA (Number of False Alarms) algorithm is used to filter line features, resulting in extracted line features with good scale and rotation invariance. Line feature description and matching are performed using the LBD (Line Band Descriptor) algorithm and the KNN (K-Nearest Neighbor) algorithm, respectively. For area features, the PCA (Principal Components Analysis) plane fitting algorithm is used for area feature extraction. Image patches with significant missing points or depth discontinuities are divided into non-planar regions. Robust matching of area features is achieved by performing a Mahalanobis distance test between the normal vectors of candidate matches, utilizing the uncertainty of the current state and the uncertainty of planar points.

[0038] (3) Robust Feature Extraction for Millimeter-Wave Radar: Before performing feature extraction on millimeter-wave radar, it is necessary to remove the Doppler effect. To compensate for the Doppler effect, we need to know the linear velocity of the sensor. This can be obtained from an IMU or odometer. Figure 4 As shown, the movement of the sensor causes a significant relative velocity between the sensor and its surroundings. This relative velocity causes the received frequency to change according to the Doppler effect. It is important to note that only the radial component of the velocity is considered. This will cause a Doppler frequency shift. Skolnik provides an expression for the Doppler frequency. ,in The wavelength of the signal. For an object moving towards the radar ( >0), the Doppler frequency is positive, resulting in a higher receiving frequency. Let ,in This refers to the transmission frequency. To correct for Doppler distortion, the distance to each target needs to be corrected using the following formula: (5) After Doppler correction, feature detection based on constant false alarm rate (CFAR) is a common method for millimeter-wave radar feature extraction. CFAR is designed to estimate the local noise floor and capture relative peaks in millimeter-wave radar data. One-dimensional CFAR can be applied to Navtech data by convolving each azimuth angle with a sliding window detector. However, CFAR generates many redundant keypoints, is difficult to optimize, and can produce false alarms due to noise artifacts in millimeter-wave radar. Therefore, a Gaussian filter should be used instead of a binomial filter, and the average value for each azimuth angle should be calculated instead of using a median filter. Furthermore, multipath reflections should be preserved to reduce redundant keypoints. After feature extraction, the raw radar scan results output from the Navtech sensor are converted from polar coordinates to Cartesian coordinates, and an ORB descriptor is calculated for each keypoint on the Cartesian image. After completing the millimeter-wave radar feature extraction, inter-frame feature matching is achieved through ORB descriptor matching, following the same process as image feature point matching.

[0039] (4) Robust feature extraction from infrared cameras: Infrared images have a lot of noise, and traditional feature extraction algorithms do not have sufficient robustness. Therefore, an accurate and lightweight neural network is used to perform corner detection and optical flow estimation on infrared images.

[0040] Step 2: Robust Feature Matching Based on Semantic Information Constraints After initial feature matching, some mismatches still exist. Therefore, semantic segmentation can be used to remove mismatches and prevent them from interfering with the final optimization process. The purpose of semantic segmentation is to classify each pixel or 3D point. The steps for removing 2D-2D mismatches are: 1) Perform ORB feature extraction and matching on the 2D image; 2) Perform semantic segmentation on the matched image and remove matches with inconsistent categories; 3) Use the RANSAC algorithm again to further filter out mismatches, leaving the final matching pairs.

[0041] The steps for eliminating 2D-3D mismatches are as follows: 1) Perform feature extraction and matching on 2D images and 3D point clouds; 2) Perform semantic segmentation on the matched images and point clouds and eliminate matches with inconsistent categories; 3) Calculate the reprojection error using the PnP algorithm and filter out matching pairs with large errors.

[0042] In harsh, high-degradation environments, traditional geometric feature descriptors are prone to degradation, leading to numerous mismatches. Under the same conditions, semantic features exhibit greater stability than traditional texture and geometric features. The fusion of geometric and semantic information can effectively eliminate mismatches and improve the robustness of feature matching.

[0043] This system aims to use semantic information to assist in point cloud registration. Anestis integrates semantic information into the point cloud registration process by considering objects with different semantics. SUMA++ uses the determinism of labels to weight the residual blocks of the registration objective function. LeGo-LAOM removes vegetation points from the point cloud because they are generally unreliable. LOAM uses edge and planar features to register point clouds, achieving accurate and fast localization and mapping. This system extends this method using semantic information, with specific improvements as follows: 1) Inspired by SE-NDT, this system performs registration based on the semantic labels of features, reducing the probability of false matches. In addition, in order to model the different contributions of different semantic classes to registration (for example, semantic information such as buildings and signs is usually more stable than categories such as ground, vehicles, and vegetation), this method adds semantically related weights to the final optimization objective function.

[0044] 2) Most existing methods downsample point clouds using voxel grid filtering to improve subsequent computational efficiency. However, some small objects containing useful information are inevitably filtered out by voxel filtering. Therefore, it is necessary to perform differential downsampling of point clouds based on their semantic categories. Specifically, different sampling rates are used for different target categories, which can effectively preserve the information of small targets, such as... Figure 6 As shown.

[0045] 3) Semantic constraints are applied to the plane fitting portion of LOAM plane feature extraction. Based on the assumption that the ground plane should be parallel to the horizontal plane and perpendicular to the surface of buildings alongside the road, poorly fitted planes can be removed. (Appendix) Figure 6 This is a visual illustration of semantic-based downsampling.

[0046] Feature Extraction: This system employs the LOAM feature extraction strategy. Specifically, this method extracts edge and planar features from the point cloud and calculates the roughness of each point. (6) in Indicates the target point. For other points on the same ring. Then, use a threshold. These points are divided into edge points (roughness greater than 100%). ) and pastries (roughness less than To ensure uniform sampling, each ring is divided into... Each part is selected. Within each part, this method selects... The sharpest point is used as the edge feature. The flattest point is used as the planar feature. Finally, the edge feature points of the entire target frame are obtained. and planar feature points .

[0047] Motion estimation: To estimate the ego motion of the LiDAR, this method minimizes the distances from target edge feature points to their corresponding lines and the distances from target planar feature points to their corresponding planes. Since different semantic classes have different meanings and should contribute differently to localization, this method adds a semantically relevant weight to each loss term. Therefore, the overall optimization objective is: (7) in, It is the i-th edge feature point The distance to the line it corresponds to. It is the j-th planar feature point The distance to its corresponding plane, where l is the semantic category of the target feature point. These are semantically related weights.

[0048] First, this method uses nearest neighbor. Edge feature points in frame point cloud and planar feature points To construct the corresponding edge feature sub-map and planar feature sub-map To improve registration speed, this method is based on submaps. , and target point cloud , The semantic tags are downsampled. Figure 6 The visualization results show the downsampling strategy of our proposed method and the voxel filtering downsampling commonly used in the LOAM series. It can be seen that the proposed method better preserves the point cloud of small objects, and the semantic information of the small object point cloud is explicit and beneficial for registration. Subsequently, odometry is estimated by sub-map registration of the downsampled feature point cloud. Specifically, given an edge feature point with a semantic label l... This method selects an edge feature submap with the same semantic label l. This method uses a KD-tree to find the nearest point to the target point. Each point is used as a corresponding point. Using these... A straight line is fitted to the target point, with the distance from the target point to the corresponding line as the optimization objective. Let... , If the target point is one of two distinct points on the line, then the distance from the target point to the line can be calculated as follows: (8) Similarly, given a planar feature point This method finds its corresponding semantic submap. middle The closest point. Then, use this... This method fits a plane to a set of points. It will retain ground planes if their normal vectors are vertical, and building planes if their normal vectors are horizontal. Based on this constraint, some poorly fitted planes can be removed. Let... , , Let there be three distinct points on the plane, then point The distance to the corresponding plane is: (9) Appendix Figure 7 This is a schematic diagram of robust feature matching with semantic constraints.

[0049] Step 3: Accurate Autonomous Positioning to Resist Environmental Degradation To overcome the impact of environmental feature degradation and obtain accurate pose estimation for intelligent mobile robots or unmanned vehicles, an adaptive judgment of whether environmental features are severely degraded will be made. In some scenarios such as hilly areas where severe environmental feature degradation makes feature matching impossible, pose estimation based on motion manifolds will be used for accurate localization of intelligent mobile robots or unmanned vehicles. When environmental features are not severely degraded, a SLAM optimization framework based on continuous time trajectories will be used for accurate localization.

[0050] (1) Adaptive discrimination of environmental feature degradation For laser SLAM, lidar requires spatial geometric information to extract feature points. When feature points are lacking, the state estimation method will degenerate.

[0051] Normally, constraints should be distributed in multiple directions in space, constraining the solution from various angles, as shown below. The green dots represent solutions to the problem, confined to a small region by non-degenerate constraints. Such solutions are relatively accurate; when the constraints change slightly, the solution also changes very little, remaining confined to a local region. This is a relatively ideal solution.

[0052] If most of the constraints of a solution are approximately parallel, then they represent the degenerate directions (indicated by the blue arrows). In this case, the constraints on the solution in the degenerate directions are very poor. Considering absolute parallelism, the constraints of the solution are also parallel. In this case, the solution should be avoided. If one of the constraints shifts even slightly, the local region where the solution is located will change significantly, making such a solution unreliable.

[0053] Therefore, when constructing the pose estimation problem, eigenvalue analysis can be used to determine whether degradation exists in the state space and to determine the corresponding degradation direction.

[0054] (2) Pose estimation based on motion manifold that is resistant to environmental degradation The motion manifold is a form of physical information representing the shape of the ground surface where a robot is located, which is very helpful for pose estimation. A quadratic polynomial is used to model the motion manifold near the spatial location P: (10) in (11) Therefore, the parameters of the moving manifold are: (12) The traditional method assumes that the planar moving surface is mathematically equivalent to: or Modeling the motion manifold.

[0055] Because traditional planar assumptions cannot achieve high-precision pose estimation in complex, large-scale environments, this technique introduces a motion manifold to implicitly parameterize the terrain surface, thereby achieving a unified fusion of parameterized terrain surface and visual-inertial navigation odometry. The motion manifold is an implicit parameterized representation of the terrain surface on which the vehicle travels, based on the temporal changes in pitch, roll, and yaw attitudes during vehicle operation. For example... Figure 8 As shown, the terrain surface parameters during vehicle operation can be represented as a sequence of vehicle pitch, roll, and yaw attitudes within a specific time window.

[0056] By introducing terrain parameters expressed by motion manifolds, the surface information expressed by manifold parameters can be used to constrain the pose and odometer information of the unmanned platform in the back-end optimization framework, thereby improving the pose estimation accuracy of the system in open and degraded environments.

[0057] 1) Attitude integral assisted by motion manifold First, the time derivative of the unmanned platform's rotation attitude is: (13) in (14) To perform the integral of the rotation, we need to obtain However, mileage measurement can only provide... The third dimension element. To obtain The elements of the first two dimensions require the help of the motion manifold. For a ground-based unmanned platform on the motion manifold, the following equation holds: (15) in For antisymmetric matrices The first two lines. Moving manifold The roll and pitch angles of the ground robot are clearly defined, and they should be correlated with the rotation matrix. To maintain consistency, the gradient vector of the manifold should be parallel to... ,Right now .

[0058] Taking the time derivative of equation (4.16) yields: (16) Abbreviations are used to simplify descriptions. ,get: (17) Equation (17) contains information about There are three linear equations, of which only two are linearly independent. The unconstrained one is... The third dimension, matrix Left multiplication (18) However, The third dimension can be obtained directly from odometer measurements, and their complementarity can be used to perform 6DoF pose integration.

[0059] To complete the derivation, we need to obtain from equation (17) The elements of the first and second dimensions. First, according to equation (4.18), the following equation can be written: (19) It can be transformed into (20) here, It should be noted that the above formula uses the following equation: (twenty one) Based on the measurements from the wheel speed gauge, we can obtain: (twenty two) By integrating equation (), the complete 3DoF rotational attitude can be calculated. Then, the position can be obtained by integrating the following expression: (twenty three) The above formula incorporates information provided by wheel speedometer measurements. First dimension) and incomplete bundle ( The second and third dimensions).

[0060] The motion manifold representation implicitly defines a motion that the position integral must satisfy, namely... The representation of equation (4.16) precisely satisfies the motion manifold constraint, and it can be observed that: (twenty four) Depend on and Collinearity yields: (25) In expression (25) It is clear that the constraints imposed by the moving manifold are satisfied.

[0061] With the definitions of 3D angular velocity and linear velocity in equations (24) and (25), the system can perform 6DoF pose integration based on wheel velocimeter measurements on the moving manifold. The system can complete 6DoF pose integration by combining only the wheel velocimeter measurements, non-holonomic model constraints, and information provided by the moving manifold.

[0062] (2) Visual-inertial navigation odometry fusion pose estimation based on implicit ground information constraints of motion manifold The inputs to this module include odometer data, inertial navigation data, camera data, and odometer information such as dimensions, attitude, and distance of the intelligent mobile robot or unmanned vehicle acquired in Chapter 2. The output is a platform pose estimate. First, a parameterized representation of the ground manifold is obtained through vehicle attitude estimation. Then, visual and inertial measurements are fused to estimate the pose of the unmanned ground platform, constructing an optimized sliding window pose estimation framework. The system's workflow is shown in the table below: Table 1 Autonomous Localization Based on Motion Manifold Constraints

[0063] Backend optimization flowchart as follows Figure 9 As shown, the attitude of the unmanned ground platform should be consistent with the orientation angle of the local surface. Therefore, a motion manifold can be used to provide direct constraints on the platform's attitude. Vehicle mileage information cannot be accurately estimated for pose under complex terrain. By using a local motion manifold to parameterize the terrain surface, the mileage information can be corrected, thereby providing more accurate inter-frame pose constraints for the platform. Finally, by combining visual information to provide multi-frame constraints within the common field of view, the optimization problem of visual-inertial navigation mileage fusion pose estimation based on motion manifold constraints is thus completed.

[0064] Motion manifold constraints constrain the pose variables in the state vector. If a local motion manifold perfectly represents the manifold near a point, then the following equation holds: For all i, the following condition is satisfied. (26) here Let be a distance threshold representing the range indicated by the parameterized motion manifold. It is assumed that the poses of all keyframes within the sliding window satisfy this manifold constraint, i.e., i can be any keyframe in the state vector. Since errors are inevitably introduced during modeling, it needs to be represented as a constraint with uncertainty. The specific cost function for the explicit pose constraint of the manifold is:

[0065] in (27) This represents the variance corresponding to the translation constraints. Additionally, the moving manifold also imposes constraints on the rotational attitude: (28) in Let V be the variance corresponding to the rotation constraint, and .

[0066] (3) Autonomous localization based on continuous time trajectory SLAM optimization framework Intelligent mobile robots or autonomous vehicle perception systems contain a large number of multi-source heterogeneous sensors. Discrete-time multi-sensor fusion frameworks have very high requirements for time synchronization and heavily rely on the accuracy of time synchronization between sensors. To address the cumbersome problem of fusing asynchronous, multi-frequency, and heterogeneous sensor observations, this paper considers using continuous-time trajectories parameterized by B-splines to efficiently fuse multi-source observations while optimizing the continuous-time trajectory.

[0067] A continuous time trajectory is a continuous function of pose with respect to time. With a continuous time trajectory, the sensor pose at any point within the valid time interval can be directly retrieved, eliminating the need to interpolate observations from different sensors to the same moment based on a uniform velocity model. It is worth noting that under large accelerations, the uniform velocity model cannot accurately fit the actual motion trajectory; it is a dimensionality-reduced representation of motion. In contrast, the continuous time trajectory naturally expresses the sensor's true motion trajectory.

[0068] In state estimation problems, using 4th-order B-splines to parameterize continuous-time trajectories meets the required complexity. Furthermore, to represent rotations over continuous time, cumulative B-splines should be used. B-splines offer good local controllability; the pose at any given time on the continuous-time trajectory depends only on four nearby control points. Compared to traditional discrete-time pose estimation, continuous-time pose estimation transforms the estimation of discrete poses into the estimation of a series of spline control points.

[0069] (29) 1) Inertial Constraints: B-splines possess quadratic continuity and provide first and second-order analytic derivatives with respect to time, making the fusion of inertial observations much more convenient and eliminating the need for traditional pre-integration methods. Acceleration and angular velocity predictions can be directly obtained by differentiating the continuous-time trajectory, and these predictions can be combined with the inertial navigation system's (INS) acceleration and angular velocity observations to construct constraint terms. Simultaneously, to ensure accurate estimation of the INS bias, a bias random walk constraint term also needs to be added.

[0070] (30) 2) Visual Constraints: After extracting point and line features, the constraints for RGB and infrared cameras are consistent. For point features, it is necessary to construct point reprojection constraints between the triangulated 3D points and the back-projection of image pixels to 2D points on the normalized camera coordinate plane.

[0071] (31) Regarding line features, it's important to note that 3D lines are modeled using 6-dimensional Plück coordinates, ensuring linearity in coordinate transformations and the construction of reprojection constraints. Furthermore, Plück coordinates are overparameterized; to guarantee unconstrained optimization and easy convergence in the backend, spatial lines should be modeled using orthogonal representations during the optimization process.

[0072] (32) By combining point and line feature constraints, the robustness and accuracy of pose estimation can be effectively improved.

[0073] 3) Laser Constraints: In the feature extraction module, the laser points in the current frame have been extracted as corner points and planar points. For corner points, a point-to-line constraint is constructed for the straight line formed by the corner point of the current frame and the corner point of the local map.

[0074] (33) For a plane point, construct a point-to-plane constraint term for the plane formed by the plane point in the current frame and the plane points in the local map.

[0075] (34) 4) Millimeter wave constraint terms: Constraint terms are constructed based on the number of millimeter wave scans and the target object.

[0076] (35) Based on the constraints of each sensor, a general objective function is constructed, and the Levenberg-Marquadt gradient descent algorithm is used to optimize the B-spline control points corresponding to the feature points of each sensor and the overall pose trajectory. To ensure the accuracy of gradient descent, an adaptive approach is used to estimate the confidence interval for each optimization iteration, while ensuring the positive definiteness of the incremental equation. It is important to note that although minimizing the sum of squared L2 norms of the error terms as the objective function is intuitive, the existence of mismatches will cause the optimization algorithm to focus on adjusting a single erroneous error term. Therefore, to address erroneous data associations, Huber robust kernel functions and Cauchy robust kernel functions are introduced for different types of residual terms. This reduces the impact of erroneous data associations on the backend optimization algorithm, ultimately enabling intelligent mobile robots or unmanned vehicles to achieve accurate autonomous localization in complex outdoor environments with bumps, vibrations, and environmental degradation, even without navigation satellites.

[0077] The above technical solutions are merely exemplary embodiments of the present invention. For those skilled in the art, based on the application methods and principles disclosed in the present invention, it is easy to make various types of improvements or modifications, and not limited to the methods described in the specific embodiments of the present invention. Therefore, the methods described above are merely preferred and not restrictive.

Claims

1. A method for autonomous positioning under conditions of turbulence and environmental degradation without navigation satellites, characterized in that, Includes the following steps: S1, extract robust fusion feature primitives for anti-bump and anti-distortion properties of multi-source sensors; the multi-source sensors include multiple types such as lidar, visible light, millimeter-wave radar, and infrared cameras; S2 eliminates mismatches in feature matching by constraining semantic information, and overcomes the degradation of environmental visual and geometric features caused by severe weather and vibration. S3 adaptively judges whether environmental features are severely degraded by analyzing the eigenvalues ​​of the observation matrix; in scenarios where severe environmental feature degradation makes feature matching impossible, pose estimation is performed based on motion manifold constraints to resist environmental degradation; when environmental features are not severely degraded, an autonomous localization is performed using a SLAM optimization framework based on continuous time trajectories; finally, autonomous localization resistant to turbulence and environmental degradation is achieved without navigation satellites.

2. The method according to claim 1, characterized in that, In step S1, the method for extracting robust feature primitives of lidar includes: Point cloud data is corrected in real time using IMU inertial system information to obtain accurate pose information, and motion distortion is eliminated through rigid body pose changes. The specific steps include: ① The timestamp of each distance measurement is derived from the acquisition model based on lidar. ; ② Search for the missing data in the IMU's buffer queue. The two most recent left and right IMU data frames , It meets the following conditions: ; If not found, then search for two frames of data that meet the following conditions: . ③Solve Interpolation is performed using quaternions: ④ Solve the point cloud after distortion correction Based on this, the laser points in the current frame are all transformed to the lidar coordinate system corresponding to the start time of the frame by linear interpolation; After performing motion distortion culling on the laser point cloud, the curvature value c of each laser point is calculated based on the points to its left and right in the frame, and laser points parallel to the laser direction or those that are occluded are removed. 。 3. The method according to claim 1, characterized in that, In step S1, the method for extracting robust feature primitives for visible light vision includes: ① Integrate IMU measurement information to eliminate rolling screen distortion in the original image caused by severe bumps, vibrations from mobile robots or unmanned vehicle platforms: Based on the timestamp of the exposure center of each frame image Exposure time difference with roller blind Calculate each scan line of each frame of the image, that is, the image's scan line. Line, corresponding timestamp as follows: Based on the timestamps corresponding to each row of pixels and the velocity between adjacent frames estimated by the IMU, the pixels in each row of the image are compensated to the timestamps corresponding to the pixels in the first row of the image to remove the rolling screen distortion. ② Robust point, line, and surface feature extraction and inter-frame feature matching are performed, including: point feature extraction and matching using the ORB algorithm; line feature extraction using the improved LSD line extraction algorithm; and surface feature extraction using the PCA plane fitting algorithm.

4. The method according to claim 1, characterized in that, In step S1, the method for extracting robust feature primitives of millimeter-wave radar includes: ① Correcting distortion caused by the Doppler effect: The distance to each target is corrected using the following formula: in, , For transmission frequency, The radial component of the velocity; ②After Doppler correction, millimeter-wave radar feature extraction and inter-frame feature matching are performed.

5. The method according to claim 1, characterized in that, In step S1, the method for extracting robust feature primitives of the infrared camera includes: using a neural network to perform corner detection and optical flow estimation on the infrared image.

6. The method according to any one of claims 1-5, characterized in that, The specific method of step S2 includes: (1) Use semantic segmentation to remove mismatches, including: The steps for removing 2D-2D mismatches are as follows: 1) Perform ORB feature extraction and matching on the 2D image; 2) Perform semantic segmentation on the matched image and remove matches with inconsistent categories; 3) Use the RANSAC algorithm again to further filter out mismatches, leaving the final matching pairs. To remove 2D-3D mismatches, the steps are as follows: 1) Perform feature extraction and matching on 2D images and 3D point clouds; 2) Perform semantic segmentation on the matched images and point clouds and remove matches with inconsistent categories; 3) Calculate the reprojection error using the PnP algorithm and filter out matching pairs with large errors. (2) Using semantic information to assist point cloud registration: Registration is performed separately based on the semantic labels of the features, and semantically relevant weights are added to the final optimization objective function; Differential downsampling of point clouds is performed based on their semantic categories; Semantics is used to constrain the plane fitting part in LOAM plane feature extraction; (3) Feature extraction: The LOAM feature extraction strategy is adopted, which specifically includes: Extract edge and planar features from the point cloud and calculate the roughness of each point: in Indicates the target point. For other points on the same ring; then, use a threshold to classify these points into edge points and face points; each ring is divided into Each section; within each section, select... The sharpest point is used as the edge feature. The flattest point is used as the planar feature; finally, the edge feature points of the entire target frame are obtained. and planar feature points ; (4) Self-motion estimation of lidar: A semantically relevant weight is added to each loss function term. The overall optimization objective is: in, It is the i-th edge feature point The distance to the line it corresponds to. It is the j-th planar feature point The distance to its corresponding plane, where l is the semantic category of the target feature point. These are semantically related weights; Here, let's assume a given edge feature point with semantic label l. Select an edge feature submap with the same semantic label l ,set up , If the target point is one of two distinct points on the line, then the distance from the target point to the line is calculated as follows: set up , , Let there be three distinct points on the plane, then point The distance to the corresponding plane is: 。 7. The method according to claim 1, characterized in that, In step S3, the environmental degradation-resistant pose estimation based on motion manifold constraints specifically includes the following steps: (1) State prediction: State prediction is performed using measurements of odometer information, including prediction of pose and manifold parameters; Determine whether new image keyframes need to be inserted based on the predicted pose; (2) Status update: Add the pose of the latest keyframe to the state vector; Extracting and matching visual features from images; Construct each cost function term, including odometry information constraints, visual feature point constraints, and direct manifold constraints on pose; Perform bundle constraint adjustment based on iterative optimization to minimize the sum of cost function terms; Reparameterization of moving manifolds: Reparameterized moving manifolds based on the most recent pose estimation.

8. The method according to claim 1, characterized in that, In step S3, the autonomous localization step based on the continuous-time trajectory SLAM optimization framework uses a continuous-time trajectory parameterized by B-splines to fuse multi-source observations from asynchronous, heterogeneous, and frequency-differentiated sensors, while simultaneously optimizing the continuous-time trajectory. Specifically, this includes: (1) Select 4th-order B-splines, of which cumulative B-splines are used; (2) Construction of constraint terms, including: 1) Inertial constraint term: The predicted values ​​of acceleration and angular velocity are obtained by differentiating the continuous time trajectory, and constraint terms are constructed with the observed values ​​of acceleration and angular velocity from the inertial navigation system. At the same time, a bias random walk constraint term is added. 2) Visual constraints: After extracting point features and line features, the constraints of RGB cameras and infrared cameras are the same. For point features, point reprojection constraints are constructed between the triangulated 3D points and the back projection of image pixels onto the 2D points on the normalized camera coordinate plane. For line features, a linear spatial line is constructed to perform coordinate transformation and reprojection constraints. At the same time, the spatial line is modeled using orthogonal representation during the optimization process. Laser constraints: For corner points, construct point-to-line constraints for the straight line formed by the current frame corner point and the local map corner point; for planar points, construct point-to-plane constraints for the plane formed by the current frame planar point and the local map planar point. Millimeter-wave constraint terms: Constraint terms are constructed based on the number of millimeter-wave scans and the target object; (3) Construct the overall objective function based on the constraints of each sensor, and use the Levenberg-Marquadt gradient descent algorithm to optimize the B-spline control points corresponding to the feature points of each sensor and the overall pose trajectory.

Images