A multi-modal tightly coupled slam method based on gradient descent optimization

Abstract

A multi-modal tight coupling SLAM method based on gradient descent optimization, comprising: collecting environmental feature data in the motion process of the intelligent mobile device in real time, and determining the spatial position of the intelligent mobile device by detecting the reflective marker, to construct a time-synchronized multi-modal observation set. From the multi-modal observation set, multi-modal features are identified and extracted, and the multi-modal features are fused to obtain a unified coded joint feature set. Based on the joint feature set, an initial pose graph is constructed with multi-modal feature constraints and marker anchor constraints. An error function is called to model the multi-modal observation residual and the marker residual, and the initial pose graph is optimized by iteratively adjusting the error of the modeled estimate and the pose. The optimized initial pose graph is globally consistent, and the environmental modeling result and the pose state of the intelligent mobile device are output, solving the problem of SLAM convergence difficulty caused by narrow space and metal interference in complex scenes.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of artificial intelligence, and more particularly, relates to a multi-modal tight coupling SLAM method based on gradient descent optimization. BACKGROUND

[0002] With the development of artificial intelligence, the unmanned driving technology of intelligent vehicles has attracted widespread attention. Intelligent driving decision planning is one of the key technologies to realize the unmanned driving of intelligent vehicles. Intelligent driving decision planning refers to the planning of driving decisions by vehicles according to the perceived environmental information and pre-set goals through decision algorithms and models, wherein intelligent driving decision planning includes path planning, behavior decision making, and compliance with traffic rules, etc.

[0003] However, in complex scenes such as underground garages, if the intelligent driving vehicle cannot timely identify the environmental factors such as walls and columns in the surrounding environment that may hinder or damage the intelligent vehicle, it may be more likely to cause the intelligent vehicle to have difficulty in timely changing the driving strategy during driving. Therefore, in complex driving scenes, more modal data support is needed for environmental observation to help the intelligent driving vehicle change the driving strategy in real time through multi-modal data analysis to avoid observation missing caused by narrow space or occlusion, and thus ensure the safety of intelligent driving. SUMMARY

[0004] To solve the problems in the prior art, the purpose of the present application is to solve the above-mentioned defects, and thus a multi-modal tight coupling SLAM method based on gradient descent optimization is proposed.

[0005] The application adopts the following technical solutions.

[0006] The first aspect of the application discloses a multi-modal tight coupling SLAM method based on gradient descent optimization, which comprises:

[0007] Real-time collection of environmental feature data during the motion of the intelligent mobile device, and determination of the spatial position of the intelligent mobile device by detecting the reflective marker to construct a time-synchronized multi-modal observation set;

[0008] Identification and extraction of multi-modal features from the multi-modal observation set to perform feature fusion on the multi-modal features to obtain a unified coded joint feature set;

[0009] Based on the joint feature set, an initial pose graph is constructed with multi-modal feature constraints and marker anchor constraints;

[0010] An error function is called to model the multi-modal observation residual and the marker residual, and the initial pose graph is optimized by iteratively adjusting the error of the estimated pose and modeling;

[0011] The optimized initial pose graph is subjected to global consistency verification, and the environment modeling result and the pose state of the intelligent mobile device are output after the verification is passed.

[0012] The environment feature data includes environment image data and laser echoes, and the multi-modal features are corner points and edges identified from the environment image data, geometric features of environment entities extracted from the laser echoes, and reflective markers identified from highlight points of reflective markers.

[0013] Further, the environment feature data during the motion of the intelligent mobile device is collected in real time, and the spatial position of the intelligent mobile device is determined by detecting the reflective markers to construct a time-synchronized multi-modal observation set, including:

[0014] The camera is controlled to set the frame rate to collect the environment image data during the motion of the intelligent mobile device;

[0015] The laser ranging unit is used to rotate and emit laser at a fixed scanning frequency, and receive the reflected signal to calculate the distance and angle between the intelligent mobile device and the environment entity during the motion of the intelligent mobile device, and generate a three-dimensional point set of the laser echoes;

[0016] The inertial measurement unit is used to obtain the acceleration and angular velocity output by the body of the intelligent mobile device to calculate the inertial data of the intelligent mobile device;

[0017] A time series is established according to a uniform time step, and linear interpolation is performed on the environment image data, three-dimensional point set and inertial data according to the time series, and the three-dimensional point set is converted to the intelligent mobile device coordinate system according to the calibrated extrinsic parameter matrix.

[0018] Further, the environment feature data during the motion of the intelligent mobile device is collected in real time, and the spatial position of the intelligent mobile device is determined by detecting the reflective markers to construct a time-synchronized multi-modal observation set, including:

[0019] Threshold segmentation is performed on the environment image data to extract a pixel set with brightness exceeding a first threshold in the environment image data, and a weighted centroid of the pixel set region is calculated;

[0020] The weighted centroid is converted to a light direction vector, and the light direction vector is normalized, and candidate reflection points with reflection intensity not lower than a second threshold are screened in the three-dimensional point set;

[0021] When the included angle between the candidate reflection point and the light direction vector is within a set threshold range, the candidate reflection point is marked as a reflective marker, and the three-dimensional position coordinates of the reflective marker are obtained.

[0022] Further, the multi-modal features are identified and extracted from the multi-modal observation set to perform feature fusion on the multi-modal features to obtain a unified encoded joint feature set, including:

[0023] Based on the reflection marker observation results in the multi-modal observation set, the number of times the reflection marker appears in consecutive frames and the spatial consistency of the reflection marker between the three-dimensional point set and the environmental image data are verified to preliminarily filter the reflection marker observation results;

[0024] Edge points and corner points of an environmental entity are extracted from the three-dimensional point set, and the reflection marker coordinates in the preliminarily filtered reflection marker observation results are aligned with the edge points and corner points of the environmental entity in terms of observation time stamps to construct the unified encoded joint feature set.

[0025] Further, the initial pose graph of multi-modal feature constraints and marker anchor point constraints is constructed based on the joint feature set, including:

[0026] The motion state of the intelligent mobile device at the first time is estimated according to the inertial data in the joint feature set, and the estimated position of the intelligent mobile device at the first time is associated with the environmental image data and the three-dimensional point set at the first time;

[0027] The angular velocity of the intelligent mobile device at the first time is converted into a device pose, and the device pose is bound to the associated environmental image data and three-dimensional point set to determine the pose node of each frame of observation data to obtain a pose node set.

[0028] Further, the initial pose graph of multi-modal feature constraints and marker anchor point constraints is constructed based on the joint feature set, further including:

[0029] Matching pixels in adjacent environmental image frames are determined through feature point matching, and corresponding points in the three-dimensional point set and the pose node are aligned and projected to the environmental image coordinate system to calculate the visual residual error between the three-dimensional point set and the pose node and the matching pixels to obtain a visual constraint edge;

[0030] The wall corner points and column edge points in the joint feature set are defined in terms of geometric residual error to determine a geometric constraint edge, and the predicted pose and the pose node are compared in terms of residual error using inertial data and time step to obtain an inertial constraint edge.

[0031] Further, the initial pose graph of multi-modal feature constraints and marker anchor point constraints is constructed based on the joint feature set, further including:

[0032] Each of the reflection marker points in the preliminary filtered reflection marker observation result is represented by three-dimensional coordinates, and the three-dimensional coordinate representation of the reflection marker point is converted from a local coordinate system to a global coordinate system, and the anchor point residual is calculated according to the corresponding reflection marker point and anchor point reference in the global coordinate system, so as to generate a reflection marker constraint;

[0033] The visual constraint edge, the geometric constraint edge, the inertial constraint edge and the reflection marker constraint are merged, and the initial pose graph is generated in combination with the set of pose nodes.

[0034] Further, the error function is called to model the multi-modal observation residual and the marker residual, and the initial pose graph is optimized by iteratively adjusting the pose and the error of the modeling estimation, including:

[0035] Based on the visual residual, the geometric residual, the inertial contrast residual and the anchor point residual in the initial pose graph, an overall error function is constructed, and the error partial derivative of each node is calculated according to the overall error function, to obtain the gradient vector of each node;

[0036] Based on the gradient vector, the pose of each node in the initial pose graph is updated and normalized according to a learning rate of calibration, and the error change of the updated node pose is calculated, so that when the error change is lower than a convergence threshold, an optimized pose graph is output.

[0037] The second aspect of the present application discloses a multi-modal tightly coupled SLAM device based on gradient descent optimization, which is used to realize the multi-modal tightly coupled SLAM method based on gradient descent optimization of any one of the first aspect, and the device comprises:

[0038] A data acquisition module is configured to acquire environmental feature data in the motion process of the intelligent mobile device in real time, and determine the spatial position of the intelligent mobile device by detecting the reflective marker, so as to construct a time-synchronized multi-modal observation set.

[0039] A feature set construction module is configured to identify and extract multi-modal features from the multi-modal observation set, and perform feature fusion on the multi-modal features to obtain a unified coded joint feature set.

[0040] A pose graph construction module is configured to construct an initial pose graph of multi-modal feature constraints and marker anchor constraints based on the joint feature set.

[0041] A pose graph optimization module is configured to call an error function to model multi-modal observation residuals and marker residuals, and optimize the initial pose graph by iteratively adjusting the pose and the error of the modeling estimation.

[0042] The pose graph verification module is configured to perform global consistency verification on the optimized initial pose graph, and output an environment modeling result and a pose state of the intelligent mobile device after the verification is passed.

[0043] The environment feature data includes environment image data and laser echoes, the multi-modal features are corner points and edges identified from the environment image data, geometric features of environment entities extracted from the laser echoes, and reflective markers identified from highlight points of the reflective markers.

[0044] The third aspect of the application discloses a terminal, comprising a processor and a storage medium.

[0045] The storage medium is configured to store instructions.

[0046] The processor is configured to operate according to the instructions to perform the steps of the method of the first aspect.

[0047] The fourth aspect of the application discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method of the first aspect.

[0048] The application has the following advantages compared with the prior art:

[0049] (1) In the motion process of the vehicle-mounted robot, the camera, the laser ranging unit and the inertial measurement unit are used to collect environment feature data, the spatial position of the highlight echo or bright spot of the reflective marker is observed, and all the collected results are unified to the same time reference to ensure the consistency of the observation data, thereby providing complete original information for subsequent establishment of unified constraints, and effectively avoiding the situation that the observation missing caused by narrow space and shielding cannot be aligned.

[0050] (2) The application extracts and fuses the multi-modal observation set, identifies corner points and edges from the environment image, extracts wall and column geometric features from the laser echoes, and identifies reflective markers and their unique identifiers from the synchronous highlight points, all the features are finally unified and coded to form a joint feature set, and the identification result of the reflective marker is added to enhance the observability in the narrow and metal interference environment.

[0051] (3) The application unifies the multi-modal observation residual and the labeled residual by constructing a unified error function, uses the strong geometric convergence direction provided by the label constraint to avoid optimization from falling into local optimum in the gradient descent process, and adjusts the pose and map estimation through iteration until the error converges, thereby significantly reducing the risk of local optimum in narrow environment and under metal reflection, improving the stability of error convergence. Finally, the global consistency of the converged optimization result is checked to ensure that the map is distortion-free in the overall structure and the pose estimation is stable and reliable. If the check is passed, the final high-precision parking lot environment map and vehicle pose result are output, solving the problem of SLAM convergence difficulty caused by narrow space and metal interference in complex scenes such as underground parking lots. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is a flowchart of a multi-modal tightly coupled SLAM method based on gradient descent optimization provided by the application.

[0053] Figure 2 is a structural schematic diagram of a multi-modal tightly coupled SLAM device based on gradient descent optimization provided by the application. DETAILED DESCRIPTION

[0054] The application will be further described below with reference to the drawings. The following examples are only used to more clearly illustrate the technical solutions of the application, and cannot be used to limit the protection scope of the application.

[0055] As shown in Figure 1 , in one embodiment, a multi-modal tightly coupled SLAM method based on gradient descent optimization includes the following steps:

[0056] Step S110, real-time acquisition of environmental feature data during the movement of the intelligent mobile device, and determination of the spatial position of the intelligent mobile device by detecting the reflective marker to construct a time-synchronized multi-modal observation set.

[0057] Among them, the environmental feature data includes environmental image data and laser echo.

[0058] In some embodiments, the multi-modal tightly coupled SLAM method based on gradient descent optimization provided by the application specifically includes the following steps in step S110:

[0059] Step S111, control the camera to acquire environmental image data during the movement of the intelligent mobile device at a set frame rate.

[0060] Step S112, rotate the emitted laser at a fixed scanning frequency through the laser ranging unit, and receive the reflected signal to calculate the distance and angle between the intelligent mobile device and the environment entity during the movement of the intelligent mobile device, and generate a three-dimensional point set of laser echo.

[0061] Step S113, acquiring the acceleration and angular velocity output by the body of the intelligent mobile device through an inertial measurement unit to calculate the inertial data of the intelligent mobile device.

[0062] Step S114, establishing a time sequence according to a unified time step, and performing linear interpolation on the environmental image data, the three-dimensional point set, and the inertial data according to the time sequence, and converting the three-dimensional point set to the intelligent mobile device coordinate system according to the calibrated external parameter matrix.

[0063] In some embodiments, the application provides a multi-modal tight coupling SLAM method based on gradient descent optimization, and step S110 further includes the following steps:

[0064] Step S115, performing threshold segmentation on the environmental image data to extract a pixel set with brightness exceeding a first threshold in the environmental image data, and calculating a weighted centroid of the pixel set region.

[0065] Step S116, converting the weighted centroid into a light direction vector, normalizing the light direction vector, and screening candidate reflection points with reflection intensity not lower than a second threshold in the three-dimensional point set.

[0066] Step S117, when the angle between the candidate reflection point and the light direction vector is within a set threshold range, marking the candidate reflection point as a reflection, and acquiring the three-dimensional position coordinates of the reflection.

[0067] In specific embodiments, the application provides a multi-modal tight coupling SLAM method based on gradient descent optimization, including steps 1-5:

[0068] Step 1, synchronously collecting multi-modal sensor data and reflection marker observations.

[0069] During the movement of the vehicle-mounted robot, environmental feature data is collected using a camera, a laser ranging unit, and an inertial measurement unit, and the spatial position of the reflective marker is observed by detecting the highlight echo or bright spot. All collected results are unified to the same time reference to ensure the consistency of the observation data, provide complete original information for subsequent establishment of unified constraints, and avoid the lack of observation alignment caused by narrow space and occlusion. The following sub-steps are included:

[0070] Sub-step 1.1, camera image frame acquisition.

[0071] Specifically, the camera continuously collects scene images at a set frame rate (20-60 Hz), and each frame is attached with a time stamp. The output image frame matrix has a size of HxM, and each element in the matrix represents the brightness of the corresponding pixel, with a value range of 0-255, which is used for subsequent synchronization with point cloud and IMU (inertial measurement unit) data.

[0072] Sub-step 1.2, laser ranging point cloud collection.

[0073] Specifically, the laser unit rotates the emitted light beam at a fixed scanning frequency (5-20 Hz), receives the reflected signal and calculates the distance and angle, forming a three-dimensional point set, and outputs a point cloud matrix with a size of N x 4, each row of which contains the reflection intensity and the three-dimensional coordinates of the corresponding point. Each frame of point cloud is accompanied by a time stamp for subsequent time synchronization with the image and IMU.

[0074] Sub-step 1.3, inertial measurement data collection.

[0075] Specifically, the IMU continuously outputs acceleration and angular velocity on the vehicle-mounted robot body, and records the accurate time stamp for each data, and outputs an inertial data sequence, which contains three-axis acceleration and three-axis angular velocity, with a sampling frequency of 100-400 Hz.

[0076] Sub-step 1.4, time alignment and coordinate unification.

[0077] Specifically, based on the aforementioned output image frame matrix, point cloud matrix and inertial data sequence, a time sequence is established with a unified time step (0.005-0.02 s), and the image, point cloud and inertial data are respectively linearly interpolated according to the time stamp. Then, using the calibrated external parameter matrix, the point cloud is converted from the laser radar coordinate system to the vehicle coordinate system, i.e. the coordinate system of the vehicle-mounted robot, to ensure its consistency with the camera and IMU, and finally output the aligned multi-modal observation set.

[0078] Sub-step 1.5, reflection marker observation extraction.

[0079] Specifically, threshold segmentation is performed in the time-aligned environment image, and a pixel set with brightness greater than a set threshold (150-240) is extracted, and the weighted centroid of the corresponding area of the extracted pixel set is calculated (x, y, z) , ), the expression is:

[0080] ;

[0081] In the formula, is the image frame matrix, is the pixel coordinate.

[0082] After that, the obtained weighted centroid (x, y, z) , ) is converted into a light ray direction vector and normalized. Candidate points with a reflection intensity not lower than a set threshold (0.6-0.9) in the point cloud matrix are screened, and if the included angle between a certain candidate point and the light ray direction is within a set range, the candidate point is determined as the corresponding reflection marker, and its three-dimensional coordinates are recorded. Finally, the reflection marker observation set is output.

[0083] It should be noted that the weighted centroid (x, y) is a pixel coordinate on the image plane, which is two-dimensional information, and needs to be converted into a three-dimensional light ray direction vector. In the conversion process, the camera's intrinsic parameters (principal point coordinates and focal length) need to be used. First, the weighted centroid (x, y) pixel coordinates are subtracted from the principal point coordinates and then divided by the focal length, which can obtain the corresponding normalized coordinates. Combined with the normalized vector length of 1, the conversion of the weighted centroid (x, y) to the light ray direction vector can be completed. ) is converted into a light ray direction vector and normalized. Candidate points with a reflection intensity not lower than a set threshold (0.6-0.9) in the point cloud matrix are screened, and if the included angle between a certain candidate point and the light ray direction is within a set range, the candidate point is determined as the corresponding reflection marker, and its three-dimensional coordinates are recorded. Finally, the reflection marker observation set is output. ) pixel coordinates are subtracted from the principal point coordinates and then divided by the focal length, which can obtain the corresponding normalized coordinates. Combined with the normalized vector length of 1, the conversion of the weighted centroid (x, y) to the light ray direction vector can be completed.

[0084] In the present embodiment, the three-dimensional coordinates of the candidate points can form a direction vector in the camera coordinates. The included angle between the direction vector and the light ray direction vector is calculated by the cosine formula, and the included angle setting range is determined according to the included angle.

[0085] Step S120, identifying and extracting multi-modal features from the multi-modal observation set to perform feature fusion on the multi-modal features to obtain a unified encoded joint feature set.

[0086] Among them, the multi-modal features are corner points and edges identified from the environmental image data, geometric features of environmental entities extracted from the laser echo, and reflection markers identified from the highlight points of the reflective markers.

[0087] In some embodiments, the present application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, and step S120 specifically includes the following steps:

[0088] Step S121, based on the reflection marker observation results in the multi-modal observation set, verifying the number of times the reflection marker appears in consecutive frames and the spatial consistency of the reflection marker between the three-dimensional point set and the environmental image data, to preliminarily filter the reflection marker observation results.

[0089] Step S122, extracting edge points and corner points of environmental entities from the three-dimensional point set, and aligning the observation time stamps of the reflection marker coordinates in the preliminarily filtered reflection marker observation results and the edge points and corner points of the environmental entities, to construct a unified encoded joint feature set.

[0090] ​​​​In specific embodiments, the application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, step 2, multi-modal feature fusion and reflective marker recognition. The multi-modal observation set output in step 1 is subjected to feature extraction and fusion, corner and edge points are identified from the image, wall and column surface geometric features are extracted from the laser echo, and reflective markers and their unique identifiers are identified from the synchronized highlight points. All features are finally unified and coded to form a joint feature set. The identification result of the reflective marker is added to enhance the observability in narrow and metal interference environments. The following sub-steps are included:

[0091] Sub-step 2.1, candidate marker validity check.

[0092] Specifically, based on the aforementioned reflective marker observation set, the number of occurrences of the reflective marker in consecutive frames is checked. If it only appears once, it is removed. The spatial consistency of the reflective marker between the point cloud and the environment image is checked. If the deviation is greater than a set threshold (0.1-0.3m), it is removed. Finally, a preliminary filtered reflective marker set is obtained.

[0093] Sub-step 2.2, joint modeling of markers and environmental features.

[0094] Specifically, column edge points and wall corner points are extracted from the point cloud matrix, combined with the three-dimensional coordinates in the reflective marker observation set, and aligned according to the time stamp. The column edge points and wall corner points and the three-dimensional coordinates of the reflective markers are aligned to obtain the aligned joint feature set.

[0095] Sub-step 2.3, introduce motion constraints and generate standardized feature vectors.

[0096] Specifically, the coordinates of the feature points in the joint feature set at the same time point are bound with the inertial data sequence, so that each feature has a spatial position and a corresponding motion state, to output a multi-modal standardized feature vector set.

[0097] Step S130, based on the joint feature set, an initial pose graph is constructed based on multi-modal feature constraints and marker anchor point constraints.

[0098] In some embodiments, the application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, step S130 specifically includes the following steps:

[0099] Step S131, according to the inertial data in the joint feature set, the motion state of the intelligent mobile device at the first time is estimated, and the estimated position of the intelligent mobile device at the first time is associated with the environment image data and the three-dimensional point set at the first time.

[0100] Step S132, the angular velocity of the intelligent mobile device at the first time is converted into a device pose, and the device pose is bound with the associated environmental image data and the three-dimensional point set to determine a pose node of each frame of observation data, and a pose node set is obtained.

[0101] In some embodiments, the application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, and step S130 further comprises the following steps:

[0102] Step S133, matching pixels in adjacent environmental image frames are determined through feature point matching, and corresponding points in the three-dimensional point set and the pose node are projected into an environmental image coordinate system after alignment, so as to calculate visual residual errors between the three-dimensional point set, the pose node and the matching pixels, and obtain visual constraint edges.

[0103] Step S134, geometric residual errors of wall corner points and column edge points in the joint feature set are defined to determine geometric constraint edges, and comparison residual errors between predicted poses and the pose node are determined by using inertial data and time steps, and inertial constraint edges are obtained.

[0104] In some embodiments, the application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, and step S130 further comprises the following steps:

[0105] Step S135, three-dimensional coordinate representation is performed on each reflective marker point in the preliminary filtered reflective marker observation result, the three-dimensional coordinate representation of the reflective marker point is converted from a local coordinate system to a global coordinate system, and anchor point residual errors are calculated according to corresponding reflective marker points and anchor points in the global coordinate system, so as to generate reflective marker constraints.

[0106] Step S136, the visual constraint edges, the geometric constraint edges, the inertial constraint edges and the reflective marker constraints are merged, and an initial pose graph is generated in combination with the pose node set.

[0107] In a specific embodiment, the application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, and step 3 is initial pose graph construction and marker observation constraint introduction. Based on the joint feature set output in step 2, a pose graph containing odometer, inertial measurement, vision and laser geometric edges is constructed, and observation edges between each frame of pose and the identified reflective marker are added. This process obtains an initial pose graph containing multi-modal constraints and marker anchor point constraints, solves the problem of uneven distribution of single environmental features, and supplements the geometric convergence direction through anchor point observation. The following sub-steps are included:

[0108] Sub-step 3.1, pose node initialization.

[0109] Specifically, based on the aforementioned multimodal joint feature set, using the inertial data as the primary reference, the vehicle's motion state at the corresponding time is estimated, and the obtained vehicle position estimate is mapped one-to-one with the environmental image and point cloud set at that time. Simultaneously, the angular velocity information is converted into vehicle attitude, and the vehicle attitude is bound to other observation data at the corresponding time. In this way, each frame of observation data has a corresponding pose node, ensuring that the multimodal features are subsequently associated with the map structure.

[0110] Sub-step 3.2: Establishment of multimodal observation constraint edges.

[0111] Specifically, firstly, matching pixels are found from adjacent image frames through feature point matching. Then, the corresponding points and node poses in the point cloud are aligned and projected into the image coordinate system. The visual residual between the projected coordinates and the matching pixels is calculated. Finally, visual constraint edges are generated based on this visual residual. The calculation expression is:

[0112] ;

[0113] In the formula, For adjacent image frames at time t and time t+1 and Matching pixels; for The corresponding 3D point in the point cloud set; Let be the quaternion pose at time t; express The resulting rotation matrix; This is the projection function.

[0114] Subsequently, for wall corner points in the environmental feature set, the signed distance from the point to the plane is defined as the wall geometric residual. Then, based on the wall geometric residuals, the wall geometric constraint edges are generated. The expression for calculating the wall geometric residuals is:

[0115] ;

[0116] In the formula, These are the 3D points in the point cloud set corresponding to the corner points of the wall. This indicates the position of the point in the global coordinate system; This is the wall normal vector; Used as a reference point on the wall.

[0117] Similarly, for cylindrical edge points in the environmental feature set, the difference between the radial distance from the point to the cylindrical axis and the cylindrical radius is defined as the cylindrical geometric residual, and then the cylindrical geometric constraint edge is generated from the cylindrical geometric residual.

[0118] In this embodiment, the predicted pose of the vehicle is determined using IMU data and time steps, and then compared with the node pose to obtain the inertial residual. Then, inertial constraint edges are generated based on the inertial data residuals. The expression for calculating the inertial residuals is as follows:

[0119] ;

[0120] In the formula, ( , () represents the predicted pose obtained through IMU pre-integration prediction; , () represents the node pose obtained in sub-step 3.1; This is quaternion multiplication; It is a quaternion logarithmic mapping used to transform the rotation difference into the minimum rotation vector.

[0121] Sub-step 3.3, Introduction of reflection marker anchor point constraints.

[0122] Specifically, each marker point in the aforementioned set of pre-filtered reflection markers is represented using three-dimensional coordinates, and its transformation from the local coordinate system to the global coordinate system is expressed as follows:

[0123] ;

[0124] In the formula, These are the coordinates of the reflection marker points detected in the local coordinate system; For node pose quaternions The resulting rotation matrix; Let be the three-dimensional position of the pose node at time t; This represents the position of the reflection marker in the global coordinate system.

[0125] It should be noted that the attitude quaternions are known. The expression is:

[0126] ;

[0127] In the formula, The real part represents the cosine component of the rotation; The imaginary part represents the component of rotation multiplied by the sine term; all units are dimensionless.

[0128] Then, the rotation matrix obtained from the attitude quaternion The expression is:

[0129] ;

[0130] Then, the anchor point residual is calculated by comparing the reflection marker point in the global coordinate system with the corresponding anchor point reference position (the marker installation position known in advance when the parking lot is laid out or the reference established in the system initialization stage).

[0131] Sub-step 3.4, initial pose graph generation.

[0132] Specifically, the generated multi-modal constraint set (including visual constraint edges, wall geometry constraint edges, column geometry constraint edges, and inertial constraint edges) and the reflection marker constraint set (anchor point constraint edges) are taken as a union set, and the pose node set is combined with the union set to generate an initial pose graph. Finally, the generated initial pose graph is checked for consistency: confirm that each node is connected by at least one constraint edge; the reflection marker points under the same identifier are associated with only one reference anchor point position; and delete the isolated nodes or observation data that do not meet the set geometric threshold.

[0133] Step S140, calling an error function to model the multi-modal observation residual and the marker residual, and optimizing the initial pose graph by iteratively adjusting the pose and the error of the modeled estimate.

[0134] In some embodiments, the present application provides a multi-modal tightly coupled SLAM method based on gradient descent optimization, and step S140 specifically includes the following steps:

[0135] Step S141, based on the visual residual, the geometric residual, the inertial contrast residual, and the anchor point residual in the initial pose graph, an overall error function is constructed, and the error partial derivative of each node is calculated according to the overall error function to obtain the gradient vector of each node.

[0136] Step S142, based on the gradient vector, the pose of each node in the initial pose graph is updated and normalized according to the learning rate of the calibration, and the error change of the updated node pose is calculated, so that when the error change is lower than the convergence threshold, the optimized pose graph is output.

[0137] In a specific embodiment, this invention provides a multimodal tightly coupled SLAM method based on gradient descent optimization. Step 4 involves tightly coupled iterative optimization based on gradient descent. The initial pose map output in step 3 is used as the optimization object, and a unified error function is constructed to model the multimodal observation residuals and label residuals in a unified manner. During gradient descent, the strong geometric convergence direction provided by the label constraint is utilized to avoid optimization getting trapped in local optima, and the pose and map estimation are iteratively adjusted until the error converges. By adding label observation constraints, the risk of local optima in narrow environments and under metallic reflection is significantly reduced, improving convergence stability. This includes the following sub-steps:

[0138] Sub-step 4.1: Unified modeling of the error function.

[0139] Specifically, the visual residual, wall geometric residual, cylindrical geometric residual, inertial residual, and reflection mark anchor point residual obtained above are combined into a unified overall error to obtain the overall error function.

[0140] Sub-step 4.2: Gradient calculation and direction determination.

[0141] Specifically, based on the overall error function, the partial derivative of the error function for each node is calculated to obtain a gradient vector containing the partial derivatives of the position and attitude of each node. The expression is:

[0142] ;

[0143] In the formula, This indicates the overall error's sensitivity to the unknown. This indicates the sensitivity of the overall error to attitude. In gradient calculation, the reflection-labeled residual is used to provide a stable convergence direction to suppress local optima.

[0144] Sub-step 4.3: Iterative update of pose variables.

[0145] Specifically, according to the learning rate calibrated in the experiment. (Constant, range 10) -4 ~10 -2 The pose is updated using the following expression:

[0146] ;

[0147] In the formula, ( , The updated node pose is then normalized to ensure it is a unit quaternion.

[0148] Sub-step 4.4: Convergence determination and final result generation.

[0149] Specifically, according to the updated node pose obtained in sub-step 4.3, the error change of the updated node pose is calculated, that is, the absolute value of the difference between the overall error of the updated node pose and the overall error before updating is calculated by calling the overall error function, and if the error change is less than a set threshold (constant, 10 -6 ~10 -4 ), it is determined to be converged, and if it is not converged, the iteration updating is continued.

[0150] In step S150, the optimized initial pose graph is subjected to global consistency verification, and the environmental modeling result and the pose state of the intelligent mobile device are output after the verification passes.

[0151] In a specific embodiment, the application provides a multi-modal tight coupling SLAM method based on gradient descent optimization, step 5, global consistency verification and final result output. The optimized result output in step 4 is subjected to global consistency check to ensure that the map is not distorted in the overall structure and the pose estimation is stable and reliable. If the verification passes, the final high-precision parking lot environment map and vehicle pose result are output, solving the problem of SLAM convergence difficulty caused by narrow space and metal interference in underground parking lot. The following sub-steps are included:

[0152] Sub-step 5.1, global consistency check.

[0153] Specifically, the residual error is checked on the overall map structure to confirm that the map boundary is continuous, the column and wall geometry conforms to the structural features, the pose trajectory is smooth and has no mutation. If all the check conditions are met, it is marked as “pass”, and if there is local distortion or unstable trajectory, it is marked as “not pass”.

[0154] Sub-step 5.2, final result output.

[0155] Specifically, if the verification result obtained in sub-step 5.1 is “pass”, the globally consistent high-precision map and the corresponding pose estimation are directly derived; if the verification result is “not pass”, the iteration optimization of the residual error is continued.

[0156] Next, a multi-modal tight coupling SLAM device based on gradient descent optimization provided by the application is described. The multi-modal tight coupling SLAM device based on gradient descent optimization described below can be mutually corresponding to the multi-modal tight coupling SLAM method based on gradient descent optimization described above.

[0157] As Figure 2 shown, in one embodiment, a multi-modal tight coupling SLAM device based on gradient descent optimization includes a data acquisition module, a feature set construction module, a pose graph construction module, a pose graph optimization module, and a pose graph verification module.

[0158] The data acquisition module is configured to acquire environmental feature data in real time during the movement of the intelligent mobile device, and determine the spatial position of the intelligent mobile device by detecting the reflective markers, so as to construct a time-synchronized multi-modal observation set.

[0159] The feature set construction module is configured to identify and extract multi-modal features from the multi-modal observation set, to perform feature fusion on the multi-modal features, and obtain a unified encoded joint feature set.

[0160] The pose graph construction module is configured to construct an initial pose graph based on the joint feature set, and constrain the multi-modal features and the anchor points of the markers.

[0161] The pose graph optimization module is configured to model the multi-modal observation residuals and the marker residuals by calling an error function, and optimize the initial pose graph by iteratively adjusting the error of the pose and the modeling estimation.

[0162] The pose graph verification module is configured to perform global consistency verification on the optimized initial pose graph, and output the environmental modeling result and the pose state of the intelligent mobile device after the verification is passed.

[0163] The environmental feature data includes environmental image data and laser echoes, and the multi-modal features are corner points and edges identified from the environmental image data, geometric features of environmental entities extracted from the laser echoes, and reflective markers identified from highlight points of the reflective markers.

[0164] The applicant of the present application has made a detailed description and explanation of the embodiments of the present application in combination with the drawings of the specification, but those skilled in the art should understand that the above embodiments are only preferred embodiments of the present application, and the detailed description is only to help the reader better understand the spirit of the present application, and is not a limitation on the protection scope of the present application. On the contrary, any improvement or modification based on the spirit of the present application should fall within the protection scope of the present application.

Claims

1. A multi-modal tightly coupled SLAM method based on gradient descent optimization, characterized in that, The method comprises: Real-time acquisition of environmental feature data during the movement of the intelligent mobile device, and determination of the spatial position of the intelligent mobile device by detecting the reflective marker to construct a time-synchronized multi-modal observation set; Identification and extraction of multi-modal features from the multi-modal observation set to perform feature fusion on the multi-modal features to obtain a unified encoded joint feature set; Based on the joint feature set, an initial pose graph is constructed with multi-modal feature constraints and marker anchor constraints; An error function is called to model multi-modal observation residuals and marker residuals, and the initial pose graph is optimized by iteratively adjusting the error of the modeled estimate of the pose; Global consistency checking is performed on the optimized initial pose graph, and the environmental modeling result and the pose state of the intelligent mobile device are output after the checking is passed; Wherein, the environmental feature data includes environmental image data and laser echo, and the multi-modal features are corner points and edges identified from the environmental image data, geometric features of environmental entities extracted from the laser echo, and reflective markers identified from highlight points of the reflective marker; The identification and extraction of multi-modal features from the multi-modal observation set to perform feature fusion on the multi-modal features to obtain a unified encoded joint feature set comprises: Based on the reflective marker observation results in the multi-modal observation set, verify the number of times the reflective marker appears in consecutive frames and the spatial consistency of the reflective marker between the three-dimensional point set and the environmental image data to preliminarily filter the reflective marker observation results; Extract the edge points and corner points of the environmental entities from the three-dimensional point set, and align the observation timestamps of the reflective marker coordinates in the preliminarily filtered reflective marker observation results with the edge points and corner points of the environmental entities to construct the unified encoded joint feature set.

2. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 1, wherein, The real-time acquisition of environmental feature data during the movement of the intelligent mobile device, and the determination of the spatial position of the intelligent mobile device by detecting the reflective marker to construct a time-synchronized multi-modal observation set, comprises: Control the camera to acquire environmental image data during the movement of the intelligent mobile device at a set frame rate; Rotate and emit laser at a fixed scanning frequency through the laser ranging unit, and receive the reflected signal to calculate the distance and angle between the environmental entities and the intelligent mobile device during the movement of the intelligent mobile device, and generate a three-dimensional point set of the laser echo; Obtain the acceleration and angular velocity output by the body of the intelligent mobile device through the inertial measurement unit to calculate the inertial data of the intelligent mobile device; Establish a time series according to a unified time step, and perform linear interpolation on the environmental image data, three-dimensional point set and inertial data according to the time series, and convert the three-dimensional point set to the intelligent mobile device coordinate system according to the calibrated extrinsic parameter matrix.

3. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 2, wherein, The real-time acquisition of environmental feature data during the movement of the intelligent mobile device, and the determination of the spatial position of the intelligent mobile device by detecting the reflective marker to construct a time-synchronized multi-modal observation set, further comprises: performing threshold segmentation on the environmental image data to extract a pixel set with brightness exceeding a first threshold in the environmental image data, and calculating a weighted centroid of a region of the pixel set; converting the weighted centroid into a light direction vector, normalizing the light direction vector, and screening candidate reflection points with reflection intensity not lower than a second threshold in the three-dimensional point set; when an angle between the candidate reflection point and the light direction vector is within a set threshold range, marking the candidate reflection point as a reflection, and obtaining a three-dimensional position coordinate of the reflection.

4. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 1, wherein, the constructing, based on the joint feature set, an initial pose graph with multi-modal feature constraints and labeled anchor point constraints comprises: estimating a motion state of the intelligent mobile device at a first time according to inertial data in the joint feature set, and associating an estimated position of the intelligent mobile device at the first time with environmental image data and a three-dimensional point set at the first time; converting an angular velocity of the intelligent mobile device at the first time into a device pose, and binding the device pose with the associated environmental image data and three-dimensional point set to determine a pose node of each frame of observation data, thereby obtaining a pose node set.

5. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 4, wherein, the constructing, based on the joint feature set, an initial pose graph with multi-modal feature constraints and labeled anchor point constraints further comprises: determining matching pixels in adjacent environmental image frames through feature point matching, projecting corresponding points in the three-dimensional point set and the pose node to an environmental image coordinate system after alignment, and calculating visual residual errors between the three-dimensional point set, the pose node, and the matching pixels, thereby obtaining visual constraint edges; defining geometric residual errors for wall corner points and column edge points in the joint feature set to determine geometric constraint edges, and determining comparative residual errors between a predicted pose and the pose node using inertial data and a time step, thereby obtaining inertial constraint edges.

6. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 5, wherein, the constructing, based on the joint feature set, an initial pose graph with multi-modal feature constraints and labeled anchor point constraints further comprises: representing each reflection marker point in the preliminary filtered reflection marker observation result in three-dimensional coordinates, converting the three-dimensional coordinate representation of the reflection marker point from a local coordinate system to a global coordinate system, and calculating anchor point residual errors according to corresponding reflection marker points and anchor points in the global coordinate system, thereby generating reflection marker constraints; merging the visual constraint edges, the geometric constraint edges, the inertial constraint edges, and the reflection marker constraints, and combining the initial pose graph with the pose node set.

7. The multi-modal tightly coupled SLAM method based on gradient descent optimization of claim 1, wherein, the calling an error function to model multi-modal observation residual errors and marker residual errors, and optimizing the initial pose graph through iterative adjustment of errors between poses and modeling estimates comprises: constructing an overall error function based on visual residual errors, geometric residual errors, inertial comparative residual errors, and anchor point residual errors in the initial pose graph, and calculating error partial derivatives of each node according to the overall error function, thereby obtaining gradient vectors of the nodes. Based on the gradient vector, the pose of each node in the initial pose graph is updated and normalized according to a calibrated learning rate, and the error change of the updated node pose is calculated, so that when the error change is lower than a convergence threshold, an optimized pose graph is output. 8.A terminal, comprising a processor and a storage medium; characterized in that: The storage medium is configured to store instructions; The processor is configured to operate according to the instructions to perform the steps of the multi-modal tightly coupled SLAM method based on gradient descent optimization according to any one of claims 1-7.

9. A computer-readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to implement the steps of the multi-modal tightly coupled SLAM method based on gradient descent optimization according to any one of claims 1-7.

Images