Automatic inspection method and device in complex environment, storage medium and related equipment

Abstract

The invention relates to the technical field of industrial automation, in particular to an automatic inspection method and device in a complex environment, a storage medium and related equipment, data acquired by different sensors can be subjected to space-time synchronization to improve dynamic positioning precision, and when part of the sensors fail, a function can still be maintained through synchronized redundant data, so that the reliability of the system is improved. The robustness of the system is effectively enhanced, and the data processing delay can be reduced through space-time synchronization, so that the real-time control requirement of a dynamic scene is met; locally updating the newly acquired global map according to the static characteristics and the target attitude data, thereby reducing the calculation amount, improving the accuracy of navigation positioning, and adapting to a complex environment; the current positioning error is corrected according to the target point cloud data and the target image data, so that the positioning accuracy is further improved, and the inspection path and the control instruction are generated based on the corrected positioning result and the target map, so that the operation risk is effectively reduced while automatic inspection is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial automation, and particularly relates to an automatic inspection method and device in a complex environment, a storage medium and related equipment. BACKGROUND

[0002] At present, in the field of industrial automation and intelligent operation and maintenance, robots and unmanned aerial vehicles (UAVs) are gradually replacing manual work to perform high-risk, high-frequency or high-precision tasks. For example, in the substation inspection scene, a quadruped robot carries an infrared camera and a laser radar, autonomously navigates and detects equipment temperature and abnormal sound; in the warehouse logistics scene, a UAV group cooperates to count high-position shelves, and an AGV (Automated Guided Vehicle) robot sorts goods; in the disaster rescue scene, a UAV searches for survivors, and a ground robot enters a collapsed building to deliver supplies. These scenes rely on autonomous perception, real-time decision-making and precise control of robots or UAVs, but due to the complexity and diversity of the environment (such as dynamic obstacles, complex terrain and multi-source interference), the system faces serious challenges when performing tasks.

[0003] For example, when the existing quadruped robot performs automatic inspection of substation equipment, the following problems occur due to the complex environment of the substation:

[0004] 1. Data cannot be uniformly processed due to the diversity of sensors;

[0005] 2. The inspection task cannot be completed due to inaccurate positioning;

[0006] 3. The robot cannot detect the set target after navigating to the position, and thus cannot normally perform inspection;

[0007] 4. There is a risk of operation due to untimely avoidance of dynamic obstacles during operation;

[0008] 5. Motion control and navigation fail due to overly complex terrain.

[0009] In summary, the existing intelligent operation equipment cannot perform precise navigation in a complex environment, and thus cannot complete automatic inspection work and has certain operation risks. SUMMARY

[0010] The present application aims to at least solve one of the above technical defects, particularly the technical defect that the intelligent operation equipment in the prior art cannot perform precise navigation in a complex environment, and thus cannot complete automatic inspection work and has certain operation risks.

[0011] The present application provides an automatic inspection method in a complex environment, which comprises:

[0012] acquire target point cloud data, target image data and target attitude data collected by a plurality of pre-integrated sensors at the same time and in the same space during the inspection;

[0013] decouple feature points in the target point cloud data and the target image data, and perform local update on a latest acquired global map according to the decoupled target point cloud data, target image data and target attitude data, to obtain a target map containing the inspection target;

[0014] correct a current positioning error according to the target point cloud data and the target image data, and generate an inspection path and a control instruction based on the corrected positioning result and the target map, and perform an inspection operation according to the inspection path and the control instruction.

[0015] Optionally, each of the sensors at least includes a laser radar, a vision sensor and an inertial measurement unit.

[0016] The acquisition of the target point cloud data, the target image data and the target attitude data collected by the plurality of pre-integrated sensors at the same time and in the same space includes:

[0017] determining a global clock signal, and triggering the laser radar, the vision sensor and the inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through the global clock signal;

[0018] aligning the original attitude data with time stamps of the original point cloud data or the target image data to obtain target attitude data;

[0019] acquiring a coordinate transformation matrix between the laser radar and the vision sensor, and performing spatial alignment on the original point cloud data and the target image data according to the coordinate transformation matrix to obtain target point cloud data.

[0020] Optionally, the determination of the global clock signal includes:

[0021] generating a unified time stamp of the multi-sensor synchronization through a pre-configured time synchronization model, and generating a global clock signal according to the unified time stamp;

[0022] Alternatively, a unified clock signal is generated through a specific chip, the unified clock signal is input into a pre-configured time synchronization model, a unified time stamp of the multi-sensor synchronization is generated through the time synchronization model, and a global clock signal is generated according to the unified time stamp.

[0023] Optionally, the generation of the unified time stamp of the multi-sensor synchronization through the pre-configured time synchronization model includes:

[0024] determining an absolute time at which the laser radar completes a frame of point cloud, a calibration time delay between the laser radar and the vision sensor, and a three-dimensional angular velocity vector measured by the inertial measurement unit and an angular velocity compensation coefficient;

[0025] inputting the absolute time, the calibration time delay, the three-dimensional angular velocity vector and the angular velocity compensation coefficient into a pre-configured time synchronization model, and generating a unified timestamp after multi-sensor synchronization by the time synchronization model.

[0026] Optionally, after aligning the original pose data with the timestamps of the original point cloud data or the target image data, target pose data is obtained, comprising:

[0027] determining the timestamps corresponding to each visual key frame in the original point cloud data or the target image data, and interpolating the original pose data according to each timestamp to obtain virtual pose data at each timestamp;

[0028] determining a plurality of sub-intervals and virtual pose data of each sub-interval based on the time distribution of each timestamp, and obtaining a pre-integral quantity of each sub-interval after pre-integrating the virtual pose data of each sub-interval;

[0029] correcting the virtual pose data of each sub-interval by using the pre-integral quantity of each sub-interval to obtain target pose data.

[0030] Optionally, when the original pose data meets normal interpolation conditions, the interpolation of the original pose data according to each timestamp to obtain virtual pose data at each timestamp comprises:

[0031] obtaining quaternions at adjacent time points corresponding to each timestamp in the original pose data;

[0032] interpolating between the quaternions at adjacent time points corresponding to each timestamp by using a spherical linear interpolation method of quaternions to obtain virtual pose data at each timestamp.

[0033] Optionally, when the original pose data meets emergency interpolation conditions, the interpolation of the original pose data according to each timestamp to obtain virtual pose data at each timestamp comprises:

[0034] interpolating the original pose data at each timestamp by using a pre-set emergency interpolation mode to obtain virtual pose data at each timestamp.

[0035] Optionally, after pre-integrating the virtual pose data of each sub-interval, a pre-integral quantity of each sub-interval is obtained, comprising:

[0036] convert the virtual pose data of each sub-interval to the key frame coordinate system to obtain converted virtual pose data;

[0037] pre-integrate the converted virtual pose data in each sub-interval to obtain a pre-integration quantity of each sub-interval.

[0038] Optionally, before the virtual pose data of each sub-interval is corrected using the pre-integration quantity of each sub-interval to obtain target pose data, the method further comprises:

[0039] When the virtual pose data satisfies Lie group-Lie algebra optimization conditions, performing Lie algebra optimization on the virtual pose data of each sub-interval to make the interpolation path meet the maximum steering angle and acceleration limit, and dynamically adjusting interpolation parameters according to semantic information of each visual key frame to obtain optimized virtual pose data.

[0040] Optionally, the Lie algebra optimization on the virtual pose data of each sub-interval to make the interpolation path meet the maximum steering angle and acceleration limit, and dynamically adjust interpolation parameters according to semantic information of each visual key frame to obtain optimized virtual pose data, comprises:

[0041] using Lie algebra space linear interpolation method to interpolate the virtual pose data of each sub-interval, and in the interpolation process, dynamically adjusting interpolation weights according to semantic information of each visual key frame, using pre-integration-Kalman joint filtering architecture to optimize each virtual pose data, adjusting the interpolation path through acceleration-aware Bézier spline path, and using an asymmetric noise suppression strategy to process noise in each virtual pose data in different frequency bands to obtain optimized virtual pose data.

[0042] Optionally, the virtual pose data of each sub-interval is corrected using the pre-integration quantity of each sub-interval to obtain target pose data, comprising:

[0043] constructing a first target function with the minimum overall residual error in multi-sensor fusion as the optimization objective based on the pre-integration quantity of each sub-interval, and obtaining a first optimization result after optimizing and solving the first target function;

[0044] correcting the virtual pose data of each sub-interval according to the first optimization result to obtain target pose data.

[0045] Optionally, the feature points in the target point cloud data and the target image data are dynamically decoupled to obtain decoupled target point cloud data and target image data, comprising:

[0046] determine a dynamic probability value of each feature point in the target point cloud data and the target image data, and divide each feature point into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point;

[0047] optimize the static feature set and the dynamic feature set by using an incremental spatio-temporal joint optimization engine to obtain decoupled target point cloud data and target image data.

[0048] Optionally, the determining of the dynamic probability value of each feature point in the target point cloud data and the target image data comprises:

[0049] for each feature point in the target point cloud data and the target image data:

[0050] obtain the 3D coordinates, feature descriptor and timestamp of the feature point, and the geometric weight and semantic weight of the current environment;

[0051] determine a geometric factor of the feature point according to the 3D coordinates, feature descriptor and timestamp of the feature point, and determine a semantic factor of the feature point according to the feature descriptor of the feature point;

[0052] determine the dynamic probability value of the feature point according to the geometric factor, geometric weight, semantic factor and semantic weight of the feature point.

[0053] Optionally, the obtaining of the geometric weight and semantic weight of the current environment comprises:

[0054] obtain the total number of historical features and the number of historical static features of the current environment;

[0055] determine the geometric weight and semantic weight of the current environment according to the total number of historical features and the number of historical static features.

[0056] Optionally, the determining of the semantic factor of the feature point according to the feature descriptor of the feature point comprises:

[0057] determine whether the feature point meets a semantic factor restriction condition according to the feature descriptor of the feature point;

[0058] if yes, determine the semantic factor of the feature point according to a preset entropy value range.

[0059] Optionally, the determining of the dynamic probability value of the feature point according to the geometric factor, geometric weight, semantic factor and semantic weight of the feature point comprises:

[0060] determine whether the feature point meets a reliability evaluation condition according to the 3D coordinates, feature descriptor and timestamp of the feature point;

[0061] If yes, a depth confidence and an environmental change quantitative indicator of the feature point are determined, and a dynamic probability value of the feature point is determined according to the geometric factor, the geometric weight, the semantic factor, the semantic weight, the depth confidence and the environmental change quantitative indicator of the feature point.

[0062] Optionally, the determination of the environmental change quantitative indicator of the feature point comprises:

[0063] determining a voxel corresponding to the feature point, a voxel gradient, an occupancy state of the voxel at a previous time, an occupancy state of the voxel at a current time, and all sensor observation data from an initial time to the current time;

[0064] calculating the environmental change quantitative indicator of the feature point according to the voxel corresponding to the feature point, the voxel gradient, the occupancy state of the voxel at the previous time, the occupancy state of the voxel at the current time, and all the sensor observation data from the initial time to the current time.

[0065] Optionally, the division of the feature points into the static feature set and the dynamic feature set according to the dynamic probability values of the feature points comprises:

[0066] determining an adaptive threshold of a current environment;

[0067] dividing the feature points into the static feature set and the dynamic feature set according to the dynamic probability values of the feature points and the adaptive threshold.

[0068] Optionally, the determination of the adaptive threshold of the current environment comprises:

[0069] obtaining a total feature quantity and a dynamic feature quantity of a history of the current environment;

[0070] determining the adaptive threshold of the current environment according to the total feature quantity and the dynamic feature quantity of the history.

[0071] Optionally, the optimization of the static feature set and the dynamic feature set by the incremental spatio-temporal joint optimization engine to obtain the decoupled target point cloud data and target image data comprises:

[0072] generating a static map pose chain based on the static feature set, and determining a static residual corresponding to the static map pose chain;

[0073] generating a dynamic object trajectory set based on the dynamic feature set, and determining a dynamic residual corresponding to the dynamic object trajectory set;

[0074] determining a dynamic weight factor of a current environment;

[0075] According to the static residual error, the dynamic residual error, and the dynamic weight factor, a second objective function is constructed, and after incremental sliding window solving of the second objective function, an optimized static map pose chain and a dynamic object trajectory set are determined according to a second optimization result, so as to obtain decoupled target point cloud data and target image data.

[0076] Optionally, the determining of the dynamic weight factor of the current environment comprises:

[0077] Optionally, the determining of the dynamic weight factor of the current environment comprises:

[0078] Optionally, the determining of the dynamic weight factor of the current environment comprises:

[0079] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0080] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0081] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0082] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0083] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0084] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0085] Optionally, the local updating of the latest global map according to the decoupled target point cloud data, the target image data, and the target attitude data to obtain a target map containing the inspection target comprises:

[0086] Optionally, the correction of the current positioning error according to the target point cloud data and the target image data to obtain a corrected positioning result comprises:

[0087] Optionally, the correction of the current positioning error according to the target point cloud data and the target image data to obtain a corrected positioning result comprises:

[0088] Optionally, the correction of the current positioning error according to the target point cloud data and the target image data to obtain a corrected positioning result comprises:

[0089] Optionally, the inputting the target point cloud data and the target image data into a pre-constructed spatio-temporal-semantic alignment model to obtain an alignment result output by the spatio-temporal-semantic alignment model comprises:

[0090] determining a state variable corresponding to the target point cloud data and the target image data, and constructing a spatio-temporal-semantic joint voxel;

[0091] inputting the target point cloud data, the target image data, the state variable, and the spatio-temporal-semantic joint voxel into a pre-constructed spatio-temporal-semantic alignment model to obtain an alignment result output by the spatio-temporal-semantic alignment model.

[0092] Optionally, the correcting a current positioning error according to the alignment result until the corrected positioning error is minimized to obtain a corrected positioning result comprises:

[0093] calculating a compensation amount of the state variable based on the state variable and the alignment result, updating the state variable according to the compensation amount, and then returning to input the target point cloud data, the target image data, the state variable, and the spatio-temporal-semantic joint voxel into a pre-constructed spatio-temporal-semantic alignment model and subsequent steps until the calculated compensation amount is optimal;

[0094] taking the state variable when the compensation amount is optimal as the corrected positioning result.

[0095] Optionally, the alignment result comprises a residual value of a visual factor and a residual value of a semantic constraint factor;

[0096] The calculating a compensation amount of the state variable based on the state variable and the alignment result comprises:

[0097] determining a residual value of a laser factor according to the target point cloud data;

[0098] performing weighted summation on the residual value of the visual factor, the residual value of the semantic constraint factor, and the residual value of the laser factor to obtain a cross-modal residual;

[0099] calculating a compensation amount of the state variable according to the state variable and the cross-modal residual.

[0100] Optionally, the performing weighted summation on the residual value of the visual factor, the residual value of the semantic constraint factor, and the residual value of the laser factor to obtain a cross-modal residual comprises:

[0101] determining a scene entropy and an environmental change sensitivity of a current environment according to the target point cloud data and the target image data;

[0102] determine a visual weight corresponding to the residual value of the visual factor, a semantic weight corresponding to the residual value of the semantic constraint factor, and a laser weight corresponding to the residual value of the laser factor according to the scene entropy and the environmental change sensitivity;

[0103] perform weighted summation on the residual value of the visual factor and the corresponding visual weight, the residual value of the semantic constraint factor and the corresponding semantic weight, and the residual value of the laser factor and the corresponding laser weight to obtain a cross-modal residual.

[0104] Optionally, the corrected positioning result includes a semantic feature variable, a semantic topological constraint, and a dynamic obstacle probability distribution.

[0105] The inspection path and the control instruction are generated based on the corrected positioning result and the target map, including:

[0106] A semantic state set is constructed based on the semantic feature variable and the target map, the semantic state set is mapped onto a semantic manifold, and an optimization target is generated according to the mapping result.

[0107] A random chance constraint is constructed based on the dynamic obstacle probability distribution.

[0108] The optimization target is hierarchically optimized according to the semantic topological constraint and the random chance constraint, and the inspection path and the control instruction are determined according to the optimization result.

[0109] The application also provides an automatic inspection device in a complex environment, including:

[0110] A data acquisition module is configured to acquire target point cloud data, target image data, and target attitude data collected by a plurality of pre-integrated sensors at the same time and in the same space during inspection.

[0111] A map generation module is configured to decouple feature points in the target point cloud data and the target image data, and to update a global map acquired most recently based on the decoupled target point cloud data, target image data, and target attitude data to obtain a target map containing an inspection target.

[0112] An automatic inspection module is configured to correct a current positioning error based on the target point cloud data and the target image data, and to generate an inspection path and a control instruction based on a corrected positioning result and the target map, and to perform an inspection operation according to the inspection path and the control instruction.

[0113] The application further provides a computer readable storage medium, wherein computer readable instructions are stored in the computer readable storage medium, and the computer readable instructions are executed by one or more processors to make the one or more processors execute steps of the automatic inspection method in a complex environment according to any one of the above embodiments.

[0114] The application further provides a computer device, comprising: one or more processors, and a memory.

[0115] The memory stores computer readable instructions, and the computer readable instructions are executed by the one or more processors to execute steps of the automatic inspection method in a complex environment according to any one of the above embodiments.

[0116] From the above technical solutions, the embodiments of the application have the following advantages:

[0117] The automatic inspection method, device, storage medium and related equipment in a complex environment provided by the application can first acquire target point cloud data, target image data and target attitude data collected by a plurality of sensors integrated in advance at the same time and in the same space during inspection, so that the data collected by different sensors can be synchronized in time and space, the geometric and physical consistency of the motion trajectory is ensured, the dynamic positioning accuracy is improved, and when some sensors fail, the function can still be maintained through the synchronized redundant data, the robustness of the system is effectively enhanced, the data processing delay can be reduced through time and space synchronization, and the real-time control demand of the dynamic scene is met; then, the application can decouple dynamic and static features in the target point cloud data and the target image data, so that not only dynamic features can be identified and filtered out, but also the latest acquired global map can be locally updated according to the static features in the decoupled target point cloud data and target image data and the target attitude data, so that the map is updated in real time according to the current environment to improve the accuracy of navigation and positioning and adapt to complex environments, after the target map containing the inspection target is obtained, the application can also correct the current positioning error according to the target point cloud data and the target image data to further improve the positioning accuracy, finally, the application can generate an inspection path and a control instruction based on the corrected positioning result and the target map, and perform an inspection operation according to the inspection path and the control instruction, so as to realize automatic inspection while effectively reducing the operation risk. BRIEF DESCRIPTION OF DRAWINGS

[0118] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0119] Figure 1 A flowchart illustrating an automatic inspection method in a complex environment, provided as an embodiment of this application;

[0120] Figure 2 A schematic diagram illustrating the process of timestamp alignment of raw attitude data provided in an embodiment of this application;

[0121] Figure 3 A flowchart for decoupling the static and dynamic features of target point cloud data and target image data, provided in an embodiment of this application;

[0122] Figure 4 A flowchart illustrating the generation of inspection paths and control commands provided in this application embodiment;

[0123] Figure 5 A schematic diagram of the structure of an automatic inspection device in a complex environment provided in an embodiment of this application;

[0124] Figure 6 This is a schematic diagram of the internal structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0125] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0126] In one embodiment, such as Figure 1 As shown, Figure 1 This application provides a flowchart illustrating an automatic inspection method in a complex environment, as illustrated in an embodiment of the present application. The application provides an automatic inspection method in a complex environment, which may include:

[0127] S110: During inspection, acquire target point cloud data, target image data, and target attitude data collected by multiple pre-integrated sensors at the same time and in the same space.

[0128] In this step, during the inspection, the target point cloud data, target image data and target attitude data collected at the same time and in the same space can be acquired through the pre-integrated multiple sensors, so that the current location, device distribution and surrounding environment information can be determined according to the target point cloud data, target image data and target state data, and after planning the inspection path, the inspection path is navigated to the corresponding inspection target to perform the inspection task, so as to complete the automatic inspection process.

[0129] The timing of acquiring sensor data in the present application can be at the beginning of the inspection, or during the inspection process. For example, the present application can start the inspection when receiving the inspection task, or can acquire sensor data in real time during the inspection process and adjust the path to adapt to the complex environment. The inspection task here can be a pre-configured regular inspection task, or a newly added temporary inspection task, or a regular inspection task. The inspection task can be one or multiple. When multiple inspection tasks are received, the priority of each inspection task can be sorted, and the specific setting can be determined according to the actual situation, which is not limited here.

[0130] In addition, the complex environment here refers to a scene with high physical complexity (such as unstructured terrain and multi-scale obstacles), high dynamicity (such as moving objects and environmental mutations), multi-source interference, and high semantic complexity (such as polysemy objects and hidden risks).

[0131] When the present application performs the inspection work, the target point cloud data, target image data and target attitude data collected by the pre-integrated multiple sensors at the same time and in the same space can be acquired. The pre-integrated sensors of the present application include but are not limited to LiDAR, vision sensor, IMU, millimeter wave radar, Beidou navigation system, etc. The specific configuration can be determined according to the actual application environment, which is not limited here.

[0132] Among the above sensors, the LiDAR can provide high-precision three-dimensional point cloud data for environment modeling and positioning; the vision sensor can include monocular, binocular or multi-camera for image feature extraction, target recognition and depth estimation; the IMU can measure acceleration and angular velocity and provide high-frequency motion state information; the millimeter wave radar is used for long-distance obstacle detection and speed measurement, especially stable in bad weather; the Beidou navigation system can provide absolute position information to assist global positioning.

[0133] Further, in a multi-sensor system, since the data collection times of different sensors are inconsistent, if each sensor uses its own local clock, due to crystal oscillator error, temperature change and other factors, the clock frequencies of different sensors will be different, the time will gradually be out of synchronization, and the initial time between sensors will also be out of synchronization. In order to ensure the timestamp alignment of multi-source data, the data collection times of each sensor can be kept synchronized. In addition, in order to eliminate spatial position deviation, the coordinate systems of each sensor can also be unified to the same reference system, which can not only improve the data fusion accuracy and avoid feature misplacement caused by space-time deviation (such as matching error between laser point cloud and image pixel), but also enhance the system robustness, maintain the function through synchronized redundant data when part of the sensors fail (such as short-time positioning relying only on IMU and vision), optimize real-time performance by reducing data processing delay through synchronization, and meet the real-time control requirements of dynamic scenes (such as robot obstacle avoidance and path planning).

[0134] For example, when a quadruped robot needs to pass through a device area, avoid a live framework and an oil pipeline, the laser radar on the quadruped robot can scan the three-dimensional profile of the device (such as generating a point cloud skeleton of the transformer substation device by scanning); the binocular camera can identify the device nameplate (such as "#3 main transformer 101 switch"), instrument reading, insulator crack and other defects; the IMU can sense the ground inclination when the robot starts (such as cable trench cover slope), monitor the robot posture to prevent falling, and after collecting these raw data, the space-time synchronization can be performed, and then the target point cloud data, target image data and target attitude data are obtained.

[0135] S120: Decoupling the feature points in the target point cloud data and the target image data, and performing local update on the latest global map according to the decoupled target point cloud data, target image data and target attitude data to obtain a target map containing the inspection target.

[0136] In this step, after obtaining the target point cloud data, target image data and target attitude data collected by each sensor at the same time and in the same space through S110, the feature points in the target point cloud data and the target image data can be decoupled, and a static map can be constructed according to the decoupled static features and the target attitude data, and then a corresponding inspection path can be generated according to the static map.

[0137] Specifically, after obtaining the target point cloud data, the target image data and the target attitude data, since the target point cloud data and the target image data contain multi-modal features, such as the geometric features in the target point cloud data and the semantic features in the target image data, which are determined by feature extraction and target recognition on the image, etc. In this way, kinematic abnormalities can be captured through multi-modal features, and feature stability can be quantified to suppress false detection in texture repetitive areas, thereby improving the accuracy of dynamic detection.

[0138] Further, after decoupling the feature points in the target point cloud data and the target image data, dynamic feature points and static feature points are obtained. At this time, the dynamic feature points can be filtered out, and the static feature points can be used to construct a static map. For example, the application can automatically filter out mobile maintenance personnel, vehicle point clouds, etc. through dynamic and static decoupling. When it is found that the A-phase bushing thermometer is blocked by a shadow, the shadow can be eliminated. The device state change (such as the opening indicator light) can also be marked as a semantic feature. Then, the corresponding static map can be constructed using the static feature to improve the accuracy of the map.

[0139] When constructing the static map, in order to improve real-time response performance and reduce computational complexity, the application can also update the latest global map according to the decoupled target point cloud data, target image data and target attitude data, and then obtain a target map containing the inspection target. For example, the application can add static devices (mutual inductors, insulators) in the static feature to the permanent map, and automatically associate the point cloud cluster with the substation drawing coordinates by recognizing the device nameplate information (such as "main transformer B phase") through vision. When a newly installed lightning arrester is found, the local area in the latest global map can be updated. When a cable trench terrain is encountered, the target attitude data collected by the IMU can be used to correct the point cloud height, thereby further improving the accuracy of the target map.

[0140] It can be understood that the inspection target here can be a device corresponding to the inspection task, such as a circuit breaker, a disconnecting switch, etc. It can also be a temporarily discovered abnormal area. For example, the application can discover that the temperature of a joint at a certain location exceeds the standard through visual infrared temperature measurement. At this time, the abnormal area can be marked on the target map, and the high-frequency re-inspection mode can be automatically switched. The application can determine the specific location of the inspection target through the target map and achieve precise positioning.

[0141] S130: correcting the current positioning error according to the target point cloud data and the target image data, and generating an inspection path and a control instruction based on the corrected positioning result and the target map, and performing an inspection operation according to the inspection path and the control instruction.

[0142] In this step, after the target map containing the inspection target is obtained by updating the latest acquired global map in S120, the current positioning error can also be corrected according to the target point cloud data and the target image data, and the inspection path and the control instruction are generated based on the corrected positioning result and the target map. In this way, when the inspection operation is performed according to the inspection path and the control instruction, the inspection efficiency and the inspection accuracy can be further improved, and the operation risk can be reduced.

[0143] Specifically, when the current positioning error is corrected according to the target point cloud data and the target image data, the pose of the laser radar, the pose of the visual sensor, the laser-vision extrinsic parameter, and other auxiliary parameters can be determined through the target point cloud data and the target image data, so that they can be jointly optimized to reduce the positioning error. Moreover, through the multi-modal fusion positioning, the laser radar noise caused by the reflective surface such as the porcelain insulator can be solved, and the visual degradation problem when the lighting is insufficient at night can also be solved.

[0144] For example, the laser radar positioning device contour and the main transformer oil level meter position can be determined through the laser radar, the oil level scale reading can be checked and the nameplate fine adjustment position can be identified through the visual sensor. When the laser positioning of the reflective surface such as the porcelain insulator fails, the visual recognition is mainly used, when the visual degradation occurs when the lighting is insufficient at night, the laser positioning is mainly used, and in strong light, the laser contour matching is preferentially used, and in the electromagnetic interference area, the IMU magnetometer is closed, so as to improve the positioning accuracy.

[0145] After the current positioning error is corrected by the present application, the inspection path and the control instruction can be generated based on the corrected positioning result and the target map, and the inspection operation can be performed according to the inspection path and the control instruction. For example, the corrected positioning result of the present application can include the current global pose, the laser-vision extrinsic parameter, the environmental feature, etc. Therefore, the present application can generate an obstacle avoidance path (such as bypassing a temporary fence) according to the related information in the positioning result and the target map, and can also optimize the gait parameters for the anti-skid grid ground, and adjust the center of gravity height when crossing the cable trench, and stop urgently and re-plan when encountering an unexpected obstacle (dropping a tool), so as to reduce the operation risk and improve the autonomy and the intelligent degree.

[0146] In the above embodiments, during the inspection, the target point cloud data, target image data and target attitude data collected by the plurality of pre-integrated sensors at the same time and in the same space can be obtained first, so that the data collected by different sensors can be synchronized in time and space, the geometric and physical consistency of the motion trajectory is ensured, the dynamic positioning accuracy is improved, and when some sensors fail, the function can still be maintained through the synchronized redundant data, the robustness of the system is effectively enhanced, the data processing delay can be reduced through the time and space synchronization, and the real-time control demand of the dynamic scene is met; then, the feature points in the target point cloud data and the target image data can be dynamically decoupled, so that not only the dynamic features can be identified and filtered out, but also the global map newly obtained can be locally updated according to the static features in the decoupled target point cloud data, target image data and target attitude data, so that the map can be updated in real time according to the current environment to improve the accuracy of navigation and positioning, and adapt to complex environments, after the target map containing the inspection target is obtained, the positioning error can be corrected according to the target point cloud data and the target image data to further improve the positioning accuracy, finally, the inspection path and the control instruction can be generated based on the corrected positioning result and the target map, and the inspection operation can be performed according to the inspection path and the control instruction, so as to realize automatic inspection while effectively reducing the operation risk.

[0147] In one embodiment, each of the sensors can at least include a laser radar, a vision sensor and an inertial measurement unit.

[0148] In S110, the target point cloud data, target image data and target attitude data collected by the plurality of pre-integrated sensors at the same time and in the same space can be obtained, which can include:

[0149] S111: determining a global clock signal, and triggering the laser radar, the vision sensor and the inertial measurement unit to synchronously collect original point cloud data, target image data and original attitude data through the global clock signal.

[0150] S112: after aligning the original attitude data with the time stamp of the original point cloud data or the target image data, target attitude data is obtained.

[0151] S113: obtaining a coordinate transformation matrix between the laser radar and the vision sensor, and spatially aligning the original point cloud data and the target image data according to the coordinate transformation matrix to obtain target point cloud data.

[0152] In this embodiment, in order to ensure the timestamp alignment of multi-source data, the application can trigger the laser radar, visual sensor and inertial measurement unit to synchronously collect raw point cloud data, target image data and raw attitude data through a global clock signal, so as to eliminate clock drift and clock offset between different devices, make the timestamps of all sensors based on the same time reference, and correct the error caused by sensor scanning delay. In addition, since the data collection frequencies of different sensors are also different, for example, the IMU sampling rate is very high (usually 100 Hz to 1 kHz), which is much higher than that of the camera (such as 30 Hz) or the radar (such as 10 Hz). Therefore, after synchronously collecting the raw data, the application can also align the timestamps of the raw attitude data with the raw point cloud data or the target image data, so as to obtain the target attitude data.

[0153] Among them, the global clock signal of the application refers to a unified time reference signal generated by a main clock source in a multi-sensor or multi-device system, which is used to synchronize the clocks of all sub-devices (such as cameras, radars, IMUs, etc.), so as to ensure that their timestamps are strictly aligned. The global clock signal of the application can be synchronized by hardware (high precision) or software, which can be set according to actual conditions, and is not limited here.

[0154] Further, after the application aligns the timestamps of the multi-source data, in order to eliminate the spatial position deviation, the application can also unify the coordinate systems of each sensor to the same reference system, so as to improve the data fusion accuracy, avoid feature misplacement (such as matching error of laser point cloud and image pixels) caused by time and space deviation, enhance the system robustness, maintain the function through redundant data after synchronization when part of the sensors fail (such as short-time positioning relying only on IMU and vision), optimize the real-time performance, reduce the data processing delay through synchronization, and meet the real-time control demand of dynamic scene (such as robot obstacle avoidance and path planning).

[0155] For example, the application can obtain the coordinate transformation matrix between the laser radar and the visual sensor, and then align the raw point cloud data and the target image data in space according to the coordinate transformation matrix, to obtain the target point cloud data. Of course, the application can also correct the calibration parameters (such as slight deviation caused by vibration) in real time through feature matching during operation.

[0156] Further, when the application uses the pre-calibrated coordinate transformation matrix T L2C When the laser point cloud is projected onto the camera image plane to realize fusion target detection, the principle is to realize sensor coordinate system unification based on Lie Group theory. T L2CThe spatial alignment of the original point cloud data in the laser radar coordinate system and the target image data in the camera coordinate system is the key to multi-sensor fusion, is the collection of all rigid body transformations (rotation + translation) in three-dimensional space, and the specific functions include:

[0157] Coordinate unification: eliminating the spatial difference between the laser radar and the camera due to different installation positions;

[0158] Data association: projecting the laser point cloud to the image plane to assist visual feature matching;

[0159] Joint optimization: providing a geometric constraint basis for visual-laser joint positioning.

[0160] In practical applications, the rotation matrix and translation vector of the laser radar to the camera in the coordinate transformation matrix can be solved by feature point matching (such as chessboard corner points) combined with the least squares method, and the calibration accuracy of the two directly affects the effect of multi-sensor fusion. Therefore, the present application can realize the geometric consistency of cross-modal data through accurate rotation matrix and translation vector, which can also ensure that the algorithm achieves centimeter-level positioning accuracy.

[0161] In one embodiment, determining the global clock signal in S111 can include:

[0162] S1111: generating a unified timestamp of the multi-sensor after synchronization through a pre-configured time synchronization model, and generating a global clock signal according to the unified timestamp.

[0163] S1112: or, after generating a unified clock signal through a specific chip, inputting the unified clock signal into a pre-configured time synchronization model, and generating a unified timestamp of the multi-sensor after synchronization through the time synchronization model, and generating a global clock signal according to the unified timestamp.

[0164] In this embodiment, when determining the global clock signal, the present application can perform delay compensation through a pre-configured time synchronization model, thereby increasing the accuracy of the global clock signal simultaneously triggering each sensor.

[0165] Specifically, the present application can generate a unified timestamp of the multi-sensor after synchronization through a time synchronization model, and generate a global clock signal according to the unified timestamp; of course, the present application can also generate a unified clock signal through a specific chip, such as an FPGA or other special-purpose chip, and then input the unified clock signal into a pre-configured time synchronization model, generate a unified timestamp of the multi-sensor after synchronization through the time synchronization model, and then generate a global clock signal according to the unified timestamp. In this way, the multi-sensor time synchronization error can be controlled within 100μs through hardware delay compensation and dynamic angular velocity compensation, and the stability of the fusion algorithm can be ensured.

[0166] In one embodiment, the generation of the multi-sensor synchronized unified timestamp by the pre-configured time synchronization model in S1111 can include:

[0167] S11111: determining the absolute time of the completion of a frame of point cloud by the laser radar scanning, the calibration time delay between the laser radar and the vision sensor, and the three-dimensional angular velocity vector measured by the inertial measurement unit and the angular velocity compensation coefficient.

[0168] S11112: inputting the absolute time, the calibration time delay, the three-dimensional angular velocity vector and the angular velocity compensation coefficient into the pre-configured time synchronization model, and generating a multi-sensor synchronized unified timestamp by the time synchronization model.

[0169] In this embodiment, since the time synchronization model can compensate for the delay when collecting data and increase the precision of the global clock signal while triggering multiple sensors, the input data of the time synchronization model in this application includes but is not limited to the time delay of each sensor and the angular velocity delay, etc. In this way, the original timestamps of the laser radar, camera and IMU can be aligned to the same time reference for subsequent data fusion.

[0170] Specifically, the absolute time of the completion of a frame of point cloud by the laser radar scanning can be determined, which is usually recorded by the internal clock of the radar and the unit is second; the calibration time delay between the laser radar and the vision sensor can also be determined, and the calibration method can be to synchronize data collection by a checkerboard calibration board and calculate the least squares solution of the timestamp alignment error, so that the fixed time offset caused by factors such as hardware trigger signal transmission path difference and inconsistent sensor response time can be determined, and the unit is second; the three-dimensional angular velocity vector measured by the inertial measurement unit and the angular velocity compensation coefficient can also be determined.

[0171] Where the three-dimensional angular velocity vector ω IMU The mathematical expression is:

[0172] ω IMU =[ωx,ωy,ωz] T , and the norm

[0173] The above three-dimensional angular velocity vector is used to describe the instantaneous rotation angular velocity of the robot on three axes (X / Y / Z), and the unit is radian / second (rad / s). The angular velocity compensation coefficient can be fitted by a rotation experiment to linearly relate the angular velocity and the time error, and the slope after fitting is the angular velocity compensation coefficient, so that the angular velocity measured by the IMU can be converted into a proportional factor of time compensation, which is used to dynamically adjust the synchronization error, and the unit is radian / second (rad / s).

[0174] Further, the expression of the time synchronization model of the present application is:

[0175]

[0176] wherein t sync is the unified timestamp after multi-sensor synchronization, t lidar is the absolute time for laser radar to complete a frame of point cloud, Δt calib is the calibration time delay between laser radar and vision sensor, ω IMU is the three-dimensional angular velocity vector measured by the inertial measurement unit, k ω is the angular velocity compensation coefficient of the inertial measurement unit.

[0177] In addition, it should be noted that when Δt calib = 0, it means that the laser radar and the vision sensor have completed the hardware level synchronization (such as through the FPGA trigger signal, at this time only the small error caused by the dynamic angular velocity needs to be compensated; when , it means that the high-speed rotation of the robot causes the time synchronization error to increase, at this time the item plays a major compensation role; when k ω is inaccurate, it may introduce time jitter error, at this time it can be optimized through offline calibration and online adaptive filtering (such as Kalman filtering, etc.).

[0178] In one embodiment, after aligning the original pose data with the timestamps of the original point cloud data or the target image data in S112, the target pose data can include:

[0179] S1121: Determine the timestamps corresponding to each visual key frame in the original point cloud data or the target image data, and interpolate the original pose data according to each timestamp to obtain virtual pose data at each timestamp.

[0180] S1122: Determine a plurality of subintervals and virtual pose data of each subinterval based on the time distribution of each timestamp, and obtain the pre-integral quantity of each subinterval after pre-integrating the virtual pose data of each subinterval.

[0181] S1123: Correct the virtual pose data of each subinterval using the pre-integral quantity of each subinterval to obtain the target pose data.

[0182] In this embodiment, since the data sampling frequency of the IMU (usually 1 kHz) is much higher than that of the vision or laser radar (such as a camera 30 Hz), the original pose data can be aligned with the timestamps of the low-frequency sensor data through interpolation.

[0183] For example, at the moment of camera exposure, the application can achieve time consistency of multi-sensor data by interpolating the original attitude data of the IMU measurement. The interpolation method can be selected according to the actual situation, for example, the application can select the spherical linear interpolation method (SLERP) of the quaternion, or the spatial linear interpolation method of the Lie algebra, or other interpolation methods, which can be selected according to the actual situation, and is not limited here.

[0184] When the application selects a corresponding interpolation method, the time stamps corresponding to each visual key frame in the original point cloud data or target image data can be determined first, and the original attitude data is interpolated according to each time stamp and the selected interpolation method to obtain virtual attitude data at each time stamp. Then, in order to reduce the calculation amount of state estimation, the application can determine a plurality of sub-intervals and virtual attitude data of each sub-interval based on the time distribution of each time stamp, and pre-integrate the virtual attitude data of each sub-interval to obtain a pre-integration amount of each sub-interval. In this way, the pre-integration amount of each sub-interval can be used to correct the virtual attitude data of each sub-interval, and then the target attitude data is obtained. In this process, the relative motion amount is calculated in the local coordinate system by pre-integration, which can decouple the optimization problem from the initial state solution, and can compensate for the zero offset of the accelerometer and the gyroscope, and reduce the dynamic error.

[0185] In the application, the time stamps corresponding to each visual key frame are determined by the dynamic nature of the image content and system requirements. For example, when the feature matching rate between consecutive images is lower than the preset matching rate threshold (such as 60%), a new key frame can be triggered; motion saliency can also be detected: the motion energy of the image region is calculated by the optical flow method, and if it exceeds the threshold, it is marked as a key frame; time interval can also be forced to insert: if it exceeds the preset maximum interval (such as 0.5 seconds), a key frame is forced to be generated to prevent positioning loss; time stamp alignment can also be used: the key frame time stamp strictly corresponds to the camera exposure moment, and hardware synchronization (such as FPGA trigger) is used to ensure that it is consistent with the time reference of the IMU data.

[0186] Further, the pre-integration amount calculated by the application includes phase rotation, relative velocity and relative displacement, so that the application can apply relative rotation to the virtual attitude data, then correct the position according to the relative displacement, and correct the speed according to the relative velocity, and the corrected pose is taken as the motion compensation result.

[0187] In one embodiment, when the original attitude data meets the normal interpolation condition, the interpolation of the original attitude data according to each time stamp in S1121 to obtain the virtual attitude data at each time stamp can include:

[0188] S211: Obtain the quaternion of the adjacent time point corresponding to each time stamp in the original attitude data.

[0189] S212: Quaternion spherical linear interpolation is used to interpolate between quaternions corresponding to adjacent timestamps at each timestamp to obtain virtual attitude data at each timestamp.

[0190] In this embodiment, to ensure the shortest path interpolation for rotational motion and avoid gimbal lock issues that may occur with Euler angle interpolation, thus more realistically reflecting the robot's attitude changes during continuous motion, this application can select quaternion spherical linear interpolation to interpolate the original attitude data when the original attitude data meets the normal interpolation conditions. The normal interpolation conditions here can be that the angular velocity value in the original attitude data is less than the maximum angular velocity value, or other interpolation conditions; the specific conditions can be set according to the actual situation and are not limited here.

[0191] Specifically, when the original attitude data of this application meets the normal interpolation conditions, the quaternions corresponding to adjacent timestamps in the original attitude data can be obtained. Then, the quaternion spherical linear interpolation method is used to interpolate between the quaternions corresponding to adjacent timestamps at each timestamp, thus obtaining the virtual attitude data at each timestamp. During this process, this application can also normalize the interpolated quaternions to correct the initial integration conditions. Here, the quaternion refers to a simple hypercomplex number, which is composed of a real number plus an imaginary unit i, where i² = -1. Similarly, a quaternion is composed of a real number plus three imaginary units i, j, and k, i.e., of the form... The numbers a, b, c, and d are all real numbers.

[0192] In one specific implementation, this application can be implemented at adjacent time points. and between( ,in Virtual pose data is generated using SLERP (for the timestamp corresponding to one of the visual keyframes). Assume the visual keyframe in this application is located at... If triggered at a certain time, then:

[0193]

[0194] in, For timestamps The virtual pose data below, for Quaternion at time, for The quaternion at time step. The difference between this method and the original method is:

[0195] 1. Traditional linear interpolation: directly weights the four components of the quaternion linearly, resulting in distortion of the rotation axis.

[0196] 2. SLERP interpolation: Interpolating along the geodesic on the quaternion sphere, keeping the angular velocity continuous, and reducing the drift error caused by direct integration, improving the short-term accuracy of attitude estimation.

[0197] Further, the above-mentioned interpolation method of the present application can be used to dynamically adjust the interpolation step according to the sensor data frequency, for example, when the robot is moving at high speed, a denser interpolation step (such as interpolating once every 0.5ms) is adopted to avoid motion blur; it can also be used for hardware acceleration, for example, implementing parallel SLERP calculation on FPGA or GPU, using the vectorization characteristics of quaternion operations (such as simultaneously calculating multiple sets of quaternion interpolation), reducing the interpolation delay to the microsecond level; it can also be used for visual-IMU tight coupling, in visual-inertial odometry (VIO), the SLERP interpolated IMU data can be used for matching with image feature points, and the pose can be optimized jointly; or for motion compensation in dynamic environment, when there is motion distortion in laser radar scanning, the high-precision interpolated IMU data can be used to correct the point cloud coordinates in real time.

[0198] Through the above optimization, the SLERP interpolation can control the IMU attitude interpolation error within 0.01 rad (corresponding to a position error of about ±1cm) while ensuring the calculation efficiency, meeting the high-precision positioning requirements. And, by using the SLERP interpolation + pre-integration mode for timestamp alignment, the trajectory error can be reduced by 20%, especially in high-speed motion scenarios, and by decoupling the state variables through pre-integration, the optimization iteration times can be reduced by 40%, thereby meeting the real-time SLAM (Simultaneous Localization and Mapping) requirements.

[0199] In one embodiment, when the original attitude data meets the emergency interpolation condition, the interpolation of the original attitude data according to each timestamp in S1121 to obtain the virtual attitude data at each timestamp can include:

[0200] Interpolating the original attitude data at each timestamp using a preset emergency interpolation mode to obtain the virtual attitude data at each timestamp.

[0201] In this embodiment, in order to ensure the robustness of the algorithm under extreme working conditions, the present application can set a fault recovery mechanism, which can use the emergency interpolation mode for interpolation when the original attitude data meets the emergency interpolation condition. The emergency interpolation condition here can be that the angular velocity value of the original attitude data is greater than the maximum angular velocity value, or other interpolation conditions, which can be set according to actual conditions, and is not limited herein.

[0202] When the original attitude data of the present application meets the emergency interpolation condition, the present application can utilize the preset emergency interpolation mode to interpolate the original attitude data at each timestamp, thereby obtaining the virtual attitude data at each timestamp. For example, the calculation formula of the emergency interpolation mode of the present application can be:

[0203]

[0204] wherein, is the virtual attitude data at time t, is the quaternion at time t, is the quaternion at time t, is the time delay, and are two adjacent time points.

[0205] In one embodiment, after the virtual attitude data of each sub-interval is pre-integrated in S1122, the pre-integrated quantity of each sub-interval can include:

[0206] S221: converting the virtual attitude data of each sub-interval into the key frame coordinate system to obtain the converted virtual attitude data.

[0207] S222: pre-integrating the converted virtual attitude data in each sub-interval to obtain the pre-integrated quantity of each sub-interval.

[0208] In the present embodiment, when the virtual attitude data of each sub-interval is pre-integrated, the virtual attitude data of each sub-interval can be first converted into the key frame coordinate system, and then the converted virtual attitude data in each sub-interval is pre-integrated to obtain the pre-integrated quantity of each sub-interval.

[0209] For example, the process of pre-integrating the virtual attitude data of each sub-interval by the present application is as follows:

[0210] 1. Sub-interval division:

[0211] The division rule can be determined according to the time distribution of the visual key frame, and the specific strategy is as follows:

[0212] Visual key frame driven division:

[0213] The timestamp of each visual key frame corresponds to a sub-interval endpoint, i.e., the sub-interval is:

[0214]

[0215] Dynamic adjustment mechanism:

[0216] ​​If the amount of virtual pose data in a sub-interval is too small (e.g., <10 sampling points), merge with adjacent intervals to avoid integral error accumulation.

[0217] 2. Data alignment:

[0218] Convert the virtual pose data (angular velocity, acceleration) in the sub-interval to the keyframe coordinate system.

[0219] 3. Pre-integration calculation: pre-integrate the virtual pose data in each sub-interval:

[0220]

[0221] where i, j are the time indices of adjacent virtual pose data; represents the gyroscope bias (Gyroscope Bias) affecting angular velocity measurement; represents the accelerometer bias (Accelerometer Bias) affecting acceleration measurement; is the angular velocity at time k, is the acceleration at time k, is the relative rotation between times i and j, is the relative velocity between times i and j, is the relative displacement between times i and j, all of which are pre-integration quantities, is the relative rotation between times i and k, is the relative velocity between times i and k, is the relative displacement between times i and k, is the sampling time interval of the IMU, i.e., the time difference between times k and k+1, k is the IMU measurement index, ranging from i to j-1.

[0222] Through the above steps, the pre-integration quantities of each sub-interval can be obtained. By using the pre-integration quantities of each sub-interval to correct the virtual pose data of each sub-interval, the target pose data can be obtained.

[0223] In one embodiment, before the correction of the virtual pose data of each sub-interval using the pre-integration quantities of each sub-interval in S1123, it can further include:

[0224] S230: When the virtual pose data satisfies the Lie group-Lie algebra optimization condition, perform Lie algebra optimization on the virtual pose data of each sub-interval to make the interpolation path comply with the maximum steering angle and acceleration limit, and dynamically adjust the interpolation parameters according to the semantic information of each visual keyframe to obtain the optimized virtual pose data.

[0225] In this embodiment, the original pose data is interpolated by SLERP, which may exist high-frequency noise, asynchronous or data loss, which may lead to inaccurate interpolation results, especially in dynamic motion or rapid rotation. Therefore, the application proposes an interpolation optimization method, which is an adaptive kinematic constraint interpolation algorithm combined with Lie group-Lie algebra optimization. The algorithm realizes breakthrough improvement in three aspects of interpolation path optimization, dynamic parameter adjustment and noise suppression. When optimizing the interpolation path, the shortest feasible path that meets the robot dynamics can be generated based on Lie algebra parameterization. When adjusting the dynamic parameters, the interpolation weight can be adjusted in real time according to the environmental dynamics (such as obstacle speed). When suppressing noise, the IMU noise can be modeled through the covariance matrix to optimize the anti-interference of the interpolation path.

[0226] It can be understood that the Lie group-Lie algebra optimization condition here refers to the optimization condition when the virtual pose data has high-frequency noise, asynchrony or data loss, and the motion state of the robot (such as rapid rotation, dynamic acceleration) exceeds the applicable range of the conventional interpolation model, resulting in that the direct interpolation result cannot meet the kinematic constraints (such as maximum turning angle, acceleration limit) or semantic rationality (such as obstacle avoidance trajectory needs to comply with the actual scene logic). When the virtual pose data meets this optimization condition, the rotation and translation can be decoupled through Lie algebra parameterization, an optimization objective function that meets the kinematic constraints of the robot can be constructed, and the interpolation parameters (such as weight distribution, step control) can be dynamically adjusted in combination with the semantic information (such as obstacle category, motion direction) of the visual key frame, to finally generate optimized virtual pose data that meets both kinematic feasibility and environmental adaptability.

[0227] The application can use the traditional SLERP for basic interpolation to quickly generate an initial pose sequence, then use the algorithm to optimize the kinematic constraints of the initial path to correct interpolation points that do not comply with physical laws or have collision risks, and then fuse the semantic information (such as dynamic obstacle position) of the visual key frame to dynamically adjust the interpolation parameters. Moreover, in a static / low dynamic environment, the application can preferentially use SLERP to reduce computational overhead, and in a high dynamic / complex scene, the Lie group-Lie algebra optimization can be used to improve path safety and accuracy.

[0228] In one embodiment, the Lie algebra optimization of the virtual pose data of each sub-interval in S230 to make the interpolation path comply with the maximum turning angle and acceleration limit, and dynamically adjust the interpolation parameters according to the semantic information of each visual key frame to obtain the optimized virtual pose data, can include:

[0229] The Lie algebra space linear interpolation method is used to interpolate the virtual attitude data of each sub-interval, and in the interpolation process, the interpolation weight is dynamically adjusted according to the semantic information of each visual key frame, the pre-integration-Kalman filter architecture is used to optimize each virtual attitude data, the acceleration-aware Bézier spline path is used to adjust the interpolation path, and the asymmetric noise suppression strategy is used to process the noise in each virtual attitude data in different frequency bands, to obtain the optimized virtual attitude data.

[0230] In this embodiment, when optimizing the virtual attitude data of each sub-interval, the Lie algebra space linear interpolation method can be used for interpolation operation. Compared with traditional methods, the Lie algebra space linear interpolation method can better handle the interpolation problem of rotation data and avoid rotation axis distortion. In the interpolation process, considering that the environment information represented by different visual key frames in the actual scene is different, for example, some visual key frames correspond to static obstacle regions and some correspond to dynamic obstacle regions, the interpolation weight needs to be dynamically adjusted according to the semantic information of each visual key frame. For example, for the visual key frame corresponding to the dynamic obstacle region, the interpolation weight is appropriately increased to more accurately reflect the attitude change of the robot in the region.

[0231] Meanwhile, the pre-integration-Kalman filter architecture can be used to optimize each virtual attitude data. Pre-integration can calculate the relative motion in the local coordinate system, decouple the optimization problem from the initial state, compensate for the zero offset of the accelerometer and gyroscope, and reduce dynamic errors; Kalman filtering can further filter the data, reduce noise interference, and improve the accuracy and stability of the data. Through the combination of the two, the virtual attitude data can be more effectively optimized.

[0232] In addition, the acceleration-aware Bézier spline path can be used to adjust the interpolation path. The acceleration-aware Bézier spline path can smooth the interpolation path according to the acceleration information in the robot motion process, so that the interpolation path is more consistent with the actual motion trajectory of the robot, avoids the occurrence of path mutations that do not conform to the physical law, and ensures that the interpolation path conforms to the maximum turning angle and acceleration limit.

[0233] Moreover, the asymmetric noise suppression strategy can be used to process the noise in each virtual attitude data in different frequency bands. Since the noise characteristics of different frequency bands are different, the asymmetric noise suppression strategy can use different suppression methods for noise in different frequency bands, more effectively remove noise, and improve the quality of the virtual attitude data. After the above series of operations, the optimized virtual attitude data is obtained.

[0234] It can be understood that in the above interpolation optimization process, the semantic information of each visual key frame of the present application can reflect the dynamic changes of the environment in real time, such as obstacle position, obstacle speed, road flatness, robot motion state, etc. Therefore, when dynamically adjusting the interpolation weight, the robot's motion posture in a complex environment can be ensured to be more accurate and safe. For example, when the robot quickly crosses the dynamic obstacle area, by increasing the interpolation weight of the visual key frame corresponding to the area, combining the optimization of the pre-integration-Kalman joint filtering architecture, and the smoothing processing of the acceleration perception Bézier spline path, the robot can more accurately predict and respond to the motion of the obstacle, thereby avoiding collision. At the same time, the frequency band processing of the asymmetric noise suppression strategy further reduces the influence of noise on the attitude estimation, especially in high-frequency motion scenarios, which can effectively improve the anti-interference ability of the attitude data. Finally, the virtual attitude data optimized by Lie algebra not only satisfies the constraint conditions of robot dynamics, but also significantly improves the safety and accuracy of path planning, providing reliable data support for real-time SLAM and autonomous navigation.

[0235] The following takes the complete motion process of the substation inspection robot crossing the equipment area as an example to illustrate the process of using Lie algebra space linear interpolation method to interpolate the virtual attitude data of each subinterval:

[0236] Scene one:

[0237] This scene describes that the robot inspects along the preset path and needs to perform a 90° sharp turn (angular velocity suddenly changes to 6 rad / s) when passing the transformer, and the IMU produces high-frequency noise due to electromagnetic interference, and the vision system appears temporary failure due to metal reflection. At this time, the interpolation process is decomposed as follows:

[0238] 1. Pre-integration-Kalman joint filtering architecture:

[0239] When the original attitude data arrives (angular velocity ω = 6.2 rad / s, containing high-frequency noise σ = 0.3 rad / s²):

[0240] The pre-integration stage uses the following formula:

[0241]

[0242] The Kalman update stage uses the following formula:

[0243]

[0244]

[0245] wherein, is the pre-integrated corrected angular velocity increment, is the original angular velocity measurement value at time t, For the k-th sampling time, For the (k+1)th sampling time, for Time and The time interval of time, The state update value at time t (the optimal estimate after fusion of observations). The predicted state values ​​from time t-1 to time t. For acceleration compensation coefficient, Here is the Kalman gain matrix. It is a visual aid observation (automatically switches to historical trajectory prediction when it fails). This is the kinematic model prediction function for visual failure. For noise covariance, For observation models, For the transpose of the observation model, Let be the prediction error covariance from time t-1 to time t.

[0246] The effects of using the pre-integral-Kalman joint filter architecture include:

[0247] (1) High-frequency noise is filtered by sliding window: the sliding window size N=5, and outliers exceeding 3σ are filtered out.

[0248] (2) The low-frequency drift is compensated by the prediction model, and the effective angular velocity is corrected to 5.8 rad / s.

[0249] 2. Optimization of Lie group-Li algebra mapping:

[0250] The filtered pose is then transformed into Lie algebra space. The rigid body transformation matrix is ​​given by the following formula:

[0251]

[0252] in, For Lie algebra elements (6-dimensional vectors, including translation and rotation components). This is the inverse operation of the exponential mapping from Lie groups to Lie algebras. It is a special three-dimensional Euclidean group (including rotations and translations in three-dimensional space). For three-dimensional special Euclidean Lie algebras (and) (Corresponding). Through this mapping, the rotation and translation coupling problem, which is difficult to handle directly in Lie group space, is transformed into a relatively independent vector operation problem in Lie algebra space. In Lie algebra space, the filtered pose is further optimized. By utilizing the linear property of Lie algebra, the robot's posture changes can be described more accurately, avoiding undesirable phenomena such as axis distortion during rotation processing.

[0253] By the above optimization, the rotation-translation hybrid operation in the group can be decoupled into linear space operations, and the path distortion of quaternion spherical interpolation at the angular velocity mutation can be avoided (the traditional method produces an error of 0.78°). The rotation-translation hybrid operation in the group can be decoupled into linear space operations, and the path distortion of quaternion spherical interpolation at the angular velocity mutation can be avoided (the traditional method produces an error of 0.78°).

[0254] 3. Dynamic weight interpolation function:

[0255] The interpolation weight is adjusted according to the corrected angular velocity ω = 5.8 rad / s, and the formula is as follows:

[0256]

[0257]

[0258] where k is the sensitivity coefficient, is the real-time angular velocity, is the maximum angular velocity, is the minimum angular velocity, and the interpolation weight automatically transitions to the nonlinear region as the angular velocity increases. When the sensitivity coefficient k = 1.9 and e -k = 0.15, the dynamic weight w(t) = 1 / (1+0.15×5.8) = 0.534, which makes the interpolation process follow the linear region by 53.4% and enter the nonlinear correction region by 46.6%. Therefore, when > 5 rad / s, the application can automatically enhance the weight of the nonlinear term to suppress the overshoot of high-speed rotation. Compared with the fixed weight, the peak angular velocity error of the dynamic weight can be reduced by 62%.

[0259] 4. Acceleration-aware Bézier path generation:

[0260] The control points of the cubic Bézier curve are constructed, and the specific formula is as follows:

[0261]

[0262]

[0263] where, is the coordinate of the curve at parameter t, is the weight coefficient of the i-th control point, is the cubic Bernstein basis function, is the geometric angle parameter (such as the included angle of the adjacent control point line), which is used to adjust the local curvature of the curve, is the angular acceleration (second derivative), is the time or parameter interval (such as the time difference between key frames), which controls the dynamic change rate of the curve, is a weight calculation function. In order to generate an acceleration continuous and smooth path, the weight calculation function reduces the weight in the area where the angular acceleration is large, so that the curve is more gentle, and increases the weight in the area where the angular acceleration is small, allowing more flexible shape changes. The specific weight calculation function can be as follows:

[0264]

[0265] wherein, is a curvature smoothing function, is an acceleration sensitivity coefficient, is a time scaling function, is a normalization constant, and the denominator term indicates that when the angular acceleration increases, the weight will decrease, thereby inhibiting rapid direction changes. The formula is only one form of the weight calculation function of the present application. The present application can also be designed based on the acceleration constraint, or designed in an exponential decay form, etc., as long as it can meet the needs of generating an acceleration continuous and smooth path.

[0266] The control point weight coefficient calculated by the above formula can further determine the specific shape of the cubic Bézier curve. In the process of generating the curve, the curve can better adapt to the dynamic changes in the robot motion process by considering the angular acceleration information. For example, when the robot makes a sharp turn, the angular acceleration is large. At this time, by reducing the weight of the control points in the corresponding area, the curve is more gentle in this area, avoiding sudden changes in the path, thereby ensuring the smoothness of the robot motion posture. At the same time, the weight is increased in the area where the angular acceleration is small, allowing the curve to have more flexible shape changes to better fit the actual motion trajectory of the robot. The acceleration-aware Bézier spline path generated finally can effectively guide the robot to move according to the path that meets the kinematic constraints and actual scene requirements, providing a reliable basis for subsequent path optimization and autonomous navigation.

[0267] 5. Asymmetric noise suppression strategy:

[0268] High-frequency processing:

[0269]

[0270] Low-frequency processing:

[0271]

[0272] wherein, is the output signal after high-frequency noise suppression (such as filtered angular velocity or acceleration), and the subscript h indicates the result after high-frequency processing, is the measurement value of the original signal at time step (such as the angular velocity data of the IMU), For each data point within the sliding window The weights are given by where i is the index number of the data point within the sliding window. Let j represent the low-frequency noise parameters to be optimized (such as zero bias and drift), j be the index number of the original data point, and N be the total number of the original data points. This represents the actual measurement value of the j-th raw data point (such as the raw measurement value of an IMU). For the j-th reference value or ideal value, This is a projection function, whose purpose is to map the data processed by the transformation model from the original measurement space or state space to the observation space or error assessment space, so as to correlate it with the reference value. Comparison, specifically , This refers to the original measurement space or state space. For the observation space or error evaluation space, the specific form of the projection function can be flexibly designed according to different actual application scenarios and measurement models. For example, in robot posture estimation, it can be designed as a function that maps rotation matrices or quaternions to Euler angles or angular velocity spaces. Let be the transformation function, representing the low-frequency noise model. This is a robust kernel function used to suppress the effects of outliers.

[0273] As can be seen from the above strategy, this application can suppress the 0.3 rad / s² noise caused by electromagnetic pulses through 5-frame sliding window filtering; after visual recovery, the cumulative drift can also be corrected through reprojection error optimization.

[0274] Full-process effect verification:

[0275] 1. Sharp turn phase (t=1.2-1.5s):

[0276] Bézier path curvature is reduced by 40%, effectively avoiding positioning failure caused by mechanical vibration;

[0277] Dynamic weights adaptively shorten the interpolation period to 0.8ms (from 1.2ms).

[0278] 2. Noise interference stage (t=1.6-2.0s):

[0279] High-frequency filtering eliminates 83% of electromagnetic noise;

[0280] Once the vision system recovered, the reprojection optimization corrected the positional drift from 12cm to 3cm.

[0281] 3. Steady-state phase (t>2.0s):

[0282] The sensitivity coefficient k automatically recovers to 2.52 (to improve interpolation efficiency at low angular velocities).

[0283] The computation latency was reduced to 0.09ms through FPGA acceleration.

[0284] Through multi-stage optimization, the above algorithm achieves sub-centimeter-level attitude interpolation accuracy in the complex electromagnetic environment and dynamic motion scenarios of substations, thereby meeting the stringent requirements of power equipment inspection.

[0285] Scene 2:

[0286] This scenario describes a robot inspecting along a preset path when a sudden equipment displacement triggers an emergency obstacle avoidance command. The robot needs to complete a smooth interpolation from its initial pose to the target pose within 0.5 seconds, while simultaneously suppressing IMU vibration noise and visual tracking delay. The specific interpolation process is broken down as follows:

[0287] 1. Optimizing interpolation paths using Lie group-Lie algebra mappings:

[0288] Step 1.1 Transform the pose to Lie algebra space:

[0289]

[0290] Step 1.2 Construct the acceleration compensation interpolator:

[0291]

[0292] in, For dynamic interpolation weights, This is the acceleration compensation coefficient (calibrated based on the substation ground friction coefficient). For IMU in Angular acceleration measured at time t, These are the Lie algebra coordinates corresponding to the initial pose. Let be the initial pose transformation matrix. Let Lie algebra coordinates be the target pose. Let be the target pose transformation matrix. Let t be the Lie algebra coordinates of the interpolation path, and t be the current time.

[0293] Through the above optimization process, rigid motion can be converted to linear space, avoiding path distortion in quaternion spherical interpolation.

[0294] 2. Dynamic weighted interpolation function:

[0295] Real-time calculation of angular velocity sensitivity, the formula is as follows:

[0296]

[0297] in, for angular velocity sensitivity at time t, angular velocity measured by IMU at time t, angular velocity measured by IMU at time t, sensitivity coefficient (calculated from robot inertia matrix), critical angular velocity (above which nonlinear transition is activated).

[0298] In this application, when the obstacle avoidance trigger moment is detected then automatically adjust to S-curve to avoid overshoot of traditional linear interpolation.

[0299] 3. Pre-integration-Kalman filter architecture:

[0300] Pre-integration stage:

[0301]

[0302] where, is the bias-free angular velocity, is the raw angular velocity measurement at time t, is the gyroscope zero bias, is the gyroscope measurement white noise, second order term can compensate for angular acceleration effects, is the rotation delta matrix, is the matrix exponential mapping, angular acceleration measured by IMU at time t, angular acceleration measured by IMU at time t, is the kth IMU measurement time, is the k+1th IMU measurement time.

[0303] Kalman update:

[0304]

[0305]

[0306] where, is the Kalman gain matrix at the kth observation, is the prior estimation error covariance matrix at the kth observation, is the transpose of observation matrix H, R is the observation noise covariance matrix, is the prior state estimation at the kth observation, is the posterior state estimation at the kth observation, is the actual observation value at the kth observation, e.g. feature point pixel coordinates from vision, or measurement values from other sensors, For observation model function, which predicts observation value according to current state, k is the index number of observation times.

[0307] The role of the above architecture is to fuse IMU and vision data, and suppress IMU noise caused by strong electromagnetic interference of substation.

[0308] 4. Acceleration-aware Bézier spline path:

[0309] Constructing cubic Bézier curve:

[0310]

[0311]

[0312] ,

[0313] where, is the coordinate of Bézier curve at parameter t, s is the normalized parameter, is the starting point of the curve, , is the middle control point of the curve, which controls the bending direction and degree of the curve, is the end point of the curve, is the total time for walking the entire curve, t is the time parameter or path parameter, is the angular acceleration, is the inverse of angular acceleration, is the initial linear velocity, is the final linear velocity. When detecting , automatically adjust the control point to reduce the curvature of the path by 37%.

[0314] 5. Asymmetric noise suppression strategy:

[0315] High-frequency noise processing:

[0316]

[0317] where, is the output signal after high-frequency noise suppression (e.g. filtered angular velocity), representing the smoothed estimation value at time k, is the current time index, i is the time index within the sliding window, is the size of the sliding window, is the Gaussian weight coefficient, is the original measurement value at time i, is the standard deviation of the Gaussian kernel, , motor vibration noise can be suppressed.

[0318] Low-frequency drift correction:

[0319]

[0320] where, is the low frequency drift parameter vector to be optimized, is the total number of data points involved in optimization, is the index of data points involved in optimization, is the intrinsic matrix (e.g. camera intrinsic or sensor calibration matrix), is the transformation matrix associated with the th data point, is the coordinate of the th data point in the world coordinate system, is the actual observation of the th data point, is the projection function, usually representing the conversion from homogeneous coordinate to non-homogeneous coordinate, is the predicted observation.

[0321] By the above suppression strategy, the high frequency vibration (50Hz) of the motor and the temperature drift (<0.1Hz) in the substation environment can be suppressed respectively.

[0322] The complete workflow is as follows:

[0323] 1. Load the initial pose at initialization;

[0324] 2. When receiving the obstacle avoidance instruction, start the pre- integrator accumulation;

[0325] 3. Execute the following process every 5ms:

[0326] Kalman filter fusion of IMU / visual data;

[0327] Calculate the dynamic weight ;

[0328] Bézier control point dynamic update;

[0329] Generate and convert back to ;

[0330] 4. When , complete the smooth motion from to , that is, after 0.5 seconds of real-time interpolation and filter control, the actual pose of the robot coincides (or is close enough) with the target pose, and the obstacle avoidance path interpolation is completed.

[0331] ​After verifying the effectiveness of the above workflow, the algorithm can reduce the maximum angular velocity error from 2.1 rad / s to 0.8 rad / s and the average visual reprojection error from 12.3 pixels to 4.7 pixels in this scenario, with a calculation time of 0.15ms, which meets the real-time requirements of substation inspection.

[0332] This application effectively solves the dynamic interpolation problem in complex electromagnetic environments through multi-constraint joint optimization within the framework of Lie group theory, providing a high-precision motion control scheme for power line inspection robots. Experiments show that in drastic motion scenarios with angular velocities >5 rad / s, this algorithm reduces pose drift error by 60% compared to traditional methods.

[0333] In one embodiment, such as Figure 2 As shown, Figure 2 This is a schematic diagram illustrating the process of timestamp alignment of the original attitude data provided in this application embodiment; S1123 uses the pre-integral values ​​of each sub-interval to correct the virtual attitude data of each sub-interval to obtain the target attitude data, which may include:

[0334] S231: Construct a first objective function based on the pre-integrated quantities of each sub-interval, with the goal of minimizing the overall residual during multi-sensor fusion. Optimize and solve the first objective function to obtain the first optimization result.

[0335] S232: Based on the first optimization result, the virtual attitude data of each sub-interval is corrected to obtain the target attitude data.

[0336] In this embodiment, when the virtual pose data satisfies the Lie group-Lie algebra optimization conditions, this application can perform Lie algebra optimization on the virtual pose data of each sub-interval so that the interpolation path meets the maximum steering angle and acceleration limits, and dynamically adjust the interpolation parameters according to the semantic information of each visual keyframe to obtain the optimized virtual pose data.

[0337] Next, this application can construct a first objective function based on the pre-integrated quantities of each sub-interval, with the optimization objective being to minimize the overall residual during multi-sensor fusion. After optimizing and solving the first objective function, a first optimization result is obtained. Then, the virtual attitude data of each sub-interval is corrected based on the first optimization result to obtain the target attitude data.

[0338] In one specific implementation, this application can calculate the covariance matrix for each subinterval's pre-integral value. The covariance matrix can be obtained from the IMU noise parameters (gyroscope noise, accelerometer noise) through the error dynamics equation, as shown in the following formula:

[0339]

[0340] wherein, is a state transition matrix, is a noise intensity matrix.

[0341] Then, in the optimization problem, the covariance matrix can be taken as the Mahalanobis distance weight, and the Mahalanobis distance weight is optimized, and the first objective function in the optimization can adopt a least squares framework, as follows:

[0342]

[0343]

[0344] wherein, , , respectively represent the rotation residual, the velocity residual and the displacement residual in the kth sub-interval, is an information matrix (inverse of the covariance matrix) of the residual, reflecting the uncertainty of the pre-integrated quantity, n is the number of sub-intervals, and k is the sub-interval index, is a pre-integration model, and the output is a predicted relative motion quantity, is a Lie group mapping, is a kinematic constraint, is a regularization coefficient, balancing the weight of the pre-integrated residual and the kinematic constraint, and x is a summation index variable, is a rotation increment obtained by IMU pre-integration, is a velocity increment obtained by IMU pre-integration, is a pose or trajectory at time k.

[0345] The above two formulas can be combined into a unified objective function, as follows:

[0346]

[0347] wherein, is a weight balancing the Mahalanobis distance weighted residual term, is a weight balancing the kinematic constraint term (which can be determined by experiment or covariance self-adaption), is a pose sequence to be optimized, i.e., the pose transformation matrix of the robot at each time k, is a pre-integrated residual, is the sum of the Mahalanobis distance weighted rotation / velocity / displacement residuals, Kinematic constraints. In this way, not only can the optimized pose be consistent with the relative motion of the IMU measurement by minimizing the weighted sum of squares of pre-integration residuals, considering the noise distribution, but also the pre-integration residuals and the kinematic constraints can be jointly optimized to ensure the geometric consistency of the IMU data and avoid the optimization result violating the physical law.

[0348] When the application is optimized using the above formula, the weight of the high covariance interval (such as the intense motion section) can be reduced to suppress the noise influence, and the weight of the low covariance interval (such as the stationary section) can be increased to enhance the constraint effect. In addition, if the interpolation error of a sub-interval is too large (such as the covariance exceeding a threshold), re-interpolation or key frame insertion can be triggered.

[0349] In one embodiment, as shown in Figure 3 Figure 3 The flowchart provided by the embodiment of the application for decoupling the feature points in the target point cloud data and the target image data; the decoupling of the feature points in the target point cloud data and the target image data in S120 obtains the decoupled target point cloud data and target image data, which can include:

[0350] S121: determining the dynamic probability value of each feature point in the target point cloud data and the target image data, and dividing each feature point into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point.

[0351] S122: optimizing the static feature set and the dynamic feature set by using an incremental spatio-temporal joint optimization engine to obtain the decoupled target point cloud data and target image data.

[0352] In the embodiment, when the feature points in the target point cloud data and the target image data are decoupled, the dynamic probability value of each feature point can be evaluated by a dynamic probability estimation model, and then each feature point is divided into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point. Then, the application can also optimize the static feature set and the dynamic feature set by using an incremental spatio-temporal joint optimization engine, and further obtain the decoupled target point cloud data and target image data.

[0353] When the feature points in the target point cloud data and the target image data are decoupled, the traditional method only relies on geometric consistency (such as optical flow residual), which is easily disturbed by static object occlusion and light change, leading to misjudgment (such as misidentifying a reflective glass door as a dynamic object); only using semantic segmentation relies on the generalization ability of the pre-trained model, and cannot handle unknown category dynamic objects (such as suddenly appearing abnormal obstacles).

[0354] ​Based on this, when the dynamic probability estimation model is used to evaluate the dynamic probability value of each feature point, the dynamic probability value can be generated by normalizing the linear combination of geometric inconsistency and feature stability. For example, if the geometric difference is large and the feature is unstable, the output is close to 1, and it is determined as a dynamic feature; if the geometry is consistent and the feature is stable, the output is close to 0, and it is determined as a static feature, thereby effectively improving the recognition accuracy of static features and dynamic features.

[0355] Further, since the traditional method (such as VDO-SLAM) regards dynamic objects as independent rigid bodies, although the calculation complexity can be reduced, the performance can be reduced due to the problems of excessive simplification of motion, neglect of interaction, and inadaptation to deformation, thereby affecting the modeling accuracy of dynamic obstacles. Therefore, the static feature set and the dynamic feature set are optimized by using the incremental spatio-temporal joint optimization engine to ensure the consistency of the static environment and realize accurate modeling of dynamic obstacles, and finally the decoupled target point cloud data and target image data are obtained.

[0356] In one embodiment, determining the dynamic probability value of each feature point in the target point cloud data and the target image data in S121 can include:

[0357] S1211: For each feature point in the target point cloud data and the target image data: obtaining the 3D coordinates, feature descriptor and timestamp of the feature point, and the geometric weight and semantic weight of the current environment.

[0358] S1212: determining the geometric factor of the feature point according to the 3D coordinates, feature descriptor and timestamp of the feature point, and determining the semantic factor of the feature point according to the feature descriptor of the feature point.

[0359] S1213: determining the dynamic probability value of the feature point according to the geometric factor, geometric weight, semantic factor and semantic weight of the feature point.

[0360] In this embodiment, when determining the dynamic probability value of each feature point in the target point cloud data and the target image data, for each feature point in the target point cloud data and the target image data, the 3D coordinates, feature descriptor and timestamp of the feature point, and the geometric weight and semantic weight of the current environment can be obtained first, then the geometric factor of the feature point is determined according to the 3D coordinates, feature descriptor and timestamp of the feature point, and the semantic factor of the feature point is determined according to the feature descriptor of the feature point, finally the obtained parameters are input into the dynamic probability estimation model, and the dynamic probability value of the feature point can be obtained.

[0361] Wherein, the formula of the dynamic probability estimation model of the present application is as follows:

[0362]

[0363] wherein, is the dynamic probability value of the i-th feature point, is the i-th feature point object, the feature point object including the 3D coordinate, the feature descriptor and the timestamp of the i-th feature point, and i is the index number of the feature point, is a Sigmoid function, which maps any real number to the interval (0, 1), and the output value can be interpreted as a dynamic probability, is the actual displacement between adjacent frames (calculated by optical flow), is the expected projection displacement based on camera motion, is a geometric factor, is the 3D coordinate of the i-th feature point, is a preset maximum dynamic object speed, is a time change amount, determined according to the timestamp, and a is a geometric weight and b is a semantic weight, is the feature descriptor entropy value, i.e., the semantic factor, assuming that the feature descriptor of the i-th feature point is is a 256-dimensional vector (such as ORB or DeepLabv3+ output), and its entropy value is calculated as:

[0364]

[0365] wherein, is the probability value of the k-th dimension of the feature descriptor after Softmax normalization, and the greater the entropy value of the feature descriptor, the more uniform the distribution of each dimension of the descriptor, at which time the feature point lacks distinctiveness (may belong to the surface repeated texture of a dynamic object); the smaller the entropy value, the more intense the response of some dimensions of the descriptor, at which time the feature point has uniqueness (more likely to belong to a static object), is the k-th dimension feature descriptor value of the feature descriptor of the i-th feature point, is the dimension index number of the feature descriptor, is the j-th dimension feature descriptor value of the feature descriptor of the i-th feature point, is the dimension index number of the feature descriptor.

[0366] In one embodiment, the acquisition of the geometric weight and the semantic weight of the current environment in S1211 can include:

[0367] S12111: Acquire the total number of historical features and the number of historical static features of the current environment.

[0368] S12112: Determine the geometric weight and the semantic weight of the current environment according to the total number of historical features and the number of historical static features.

[0369] In this embodiment, since the geometric factor can capture kinematic abnormalities and the semantic factor can quantify feature stability, false detection of texture repetitive regions can be suppressed. Therefore, the adaptive mechanism can be used to dynamically adjust the geometric weight and the semantic weight, so as to increase the geometric weight in a structured environment (such as a corridor) to enhance sensitivity to moving objects, and increase the semantic weight in a complex texture environment (such as a forest) to suppress false judgment of static features.

[0370] Based on this, when the geometric weight and the semantic weight of the current environment are acquired, the historical total feature quantity and the historical static feature quantity of the current environment can be acquired first, and then the geometric weight and the semantic weight of the current environment are determined according to the historical total feature quantity and the historical static feature quantity. The specific formula is as follows:

[0371]

[0372] wherein, is the historical static feature quantity, is the historical total feature quantity.

[0373] After the geometric weight and the semantic weight are dynamically adjusted through the adaptive mechanism, in a warehouse environment, α can be set to 0.6 and β can be set to 0.4 to balance the forklift motion trajectory and the interference of the shelf texture; in an outdoor environment, α can be set to 0.4 and β can be set to 0.6 to preferentially rely on feature stability to resist the influence of grass and trees.

[0374] In one embodiment, determining the semantic factor of the feature point according to the feature descriptor of the feature point in S1212 can include:

[0375] S12121: determining whether the feature point meets the semantic factor restriction condition according to the feature descriptor of the feature point.

[0376] S12122: if yes, determining the semantic factor of the feature point according to the preset entropy value range.

[0377] In this embodiment, when the semantic factor of the feature point is determined, whether the feature point meets the semantic factor restriction condition can be determined according to the feature descriptor of the feature point first, and if yes, the semantic factor of the feature point is determined according to the preset entropy value range.

[0378] It can be understood that the feature descriptor of each feature point in the present application represents an abstract representation of the local area around the key point in the image or point cloud, which is used for efficient matching of corresponding features in different perspectives or time series. For example, the feature descriptor corresponding to the feature point of the target point cloud data in the present application can describe the pixel intensity or color statistics of the key point neighborhood, can capture texture features through filter responses (such as Gabor filters) or local binary patterns (LBP), and can also use a histogram of oriented gradients (HOG) to express edge and corner structures, etc. Therefore, the present application can determine whether the feature point satisfies the semantic factor restriction condition according to the feature descriptor of the feature point, and the semantic factor restriction condition can be a high dynamic object or other restriction condition. For example, for a high dynamic object (such as a flying bird), the present application can filter transient noise by limiting the entropy value range ∈[1.2, 3.0], if the feature descriptor entropy value is in this interval, it is determined to be a high dynamic feature and is given a lower semantic weight, otherwise for static features (such as wall markings) with entropy values below this range, a higher semantic weight is given. This differential processing mechanism effectively improves the robustness of feature classification in dynamic scenes. For example, in a substation inspection scene, when a feature point with an entropy value of 2.5 is detected, the system can automatically determine it to be a temporary dynamic obstacle such as a bird and filter it out during map updating; while detecting an insulator feature point with an entropy value of 0.8, it is considered as a stable static feature and included in the permanent map. This mechanism quantifies the information entropy of the feature descriptor to realize intelligent differentiation between dynamic and static objects, and provides a reliable data basis for subsequent spatio-temporal joint optimization.

[0379] In one embodiment, determining the dynamic probability value of the feature point according to the geometric factor, the geometric weight, the semantic factor and the semantic weight of the feature point in S1213 can include:

[0380] S12131: determining whether the feature point satisfies the reliability evaluation condition according to the 3D coordinates, the feature descriptor and the timestamp of the feature point.

[0381] S12132: if it is satisfied, determining the depth confidence and the environmental change quantitative index of the feature point, and determining the dynamic probability value of the feature point according to the geometric factor, the geometric weight, the semantic factor, the semantic weight, the depth confidence and the environmental change quantitative index of the feature point.

[0382] In this embodiment, when determining the dynamic probability value of the feature point, the application can first determine whether the feature point satisfies the reliability evaluation condition according to the 3D coordinates, feature descriptor and timestamp of the feature point. If it satisfies, the depth confidence and the environmental change quantitative index of the feature point are determined, and the dynamic probability value of the feature point is determined according to the geometric factor, geometric weight, semantic factor, semantic weight, depth confidence and environmental change quantitative index of the feature point. If it does not satisfy, the dynamic probability value of the feature point is directly determined according to the geometric factor, geometric weight, semantic factor and semantic weight of the feature point.

[0383] For example, the application can capture kinematic anomalies and texture features according to the 3D coordinates, feature descriptor and timestamp of the feature point. When it is determined that the feature point is the feature point of a static but low-texture object, at this time only a single feature cannot accurately evaluate the dynamic probability value of the feature point. Therefore, the application can regard the feature point satisfying the above features as satisfying the reliability evaluation condition, introduce the depth confidence as the third constraint, and introduce the environmental change quantitative index as the adjusting valve of the dynamic balance multi-sensor constraint. The specific formula is as follows:

[0384]

[0385] wherein, is the depth confidence, is the environmental change quantitative index.

[0386] The application cooperates with the depth confidence and the environmental change quantitative index, so that the SLAM system can utilize the depth information to improve the accuracy in complex scenes such as substations, and can also suppress the error caused by cross-modal interference.

[0387] In one embodiment, the determination of the environmental change quantitative index of the feature point in S12132 can include:

[0388] S321: determining the voxel corresponding to the feature point, the voxel gradient, the occupancy state of the voxel at the last time, the occupancy state of the voxel at the current time, and all sensor observation data from the initial time to the current time.

[0389] S322: calculating the environmental change quantitative index of the feature point according to the voxel corresponding to the feature point, the voxel gradient, the occupancy state of the voxel at the last time, the occupancy state of the voxel at the current time, and all sensor observation data from the initial time to the current time.

[0390] In this embodiment, when determining the environmental change quantization index of the feature point, the voxel corresponding to the feature point, the voxel gradient, the occupancy state of the voxel at the previous time, the occupancy state of the voxel at the current time, and all sensor observation data from the initial time to the current time can be determined first, and then the environmental change quantization index of the feature point is calculated according to the related information confirmed in the previous step.

[0391] For example, the calculation formula of the environmental change quantization index of the present application is as follows:

[0392]

[0393] wherein, is the i-th voxel, is the total number of voxels, i is the index number of the voxel, and the voxel corresponding to each feature point can be one or multiple, is the TSDF gradient change of the voxel , indicating the distance field change degree of the voxel between two consecutive time points, is an indicator function (1 when the occupancy state of the voxel changes, and 0 when it does not change), is the occupancy state of the voxel at the previous time, is the occupancy state of the voxel at the current time, refers to all sensor observation data (such as lidar point cloud, depth camera data) from the initial time to t-1 time, refers to all sensor observation data from the initial time to t time, is the sensor observation data at t time, when the voxel is occupied, is the probability that the sensor actually observes , is the probability that the sensor observes , is the probability of inferring that the voxel is occupied according to , is a normalization constant of the observation data, ensuring that the sum of the probabilities is 1.

[0394] The environmental change quantization index of the feature point can be calculated by the above formula. Further, in order to obtain more accurate calculation results, after obtaining multiple voxels of the feature point, the voxels can be screened according to the voxel confidence of each voxel to further optimize the calculation results.

[0395] In one embodiment, the step of dividing the feature points into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point in S121 can include:

[0396] S1214: Determine the adaptive threshold value of the current environment.

[0397] S1215: Divide each feature point into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point and the adaptive threshold value.

[0398] In this embodiment, after obtaining the dynamic probability value of each feature point, each feature point can be divided into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point, as follows:

[0399] Static feature set

[0400] Dynamic feature set

[0401] In the above formula, represents the static feature set, represents the dynamic feature set, represents the i-th feature point object, represents the dynamic probability value of the i-th feature point, and the Threshold in the static feature set and the dynamic feature set is the adaptive threshold value. In robot dynamic positioning and alignment technology, the adaptive threshold value is a key parameter for achieving high precision and robustness of the algorithm. However, since the adaptive threshold value in traditional algorithms is a fixed value, it cannot distinguish between dynamic and static features, and may result in false matching. When the set threshold is too high, a high threshold will also result in missing low-speed dynamic objects. According to the current environment, the adaptive threshold value of the present application can be automatically increased (up to 0.25) when the scene is highly dynamic (such as a dense crowd), thereby avoiding excessive sensitivity, and automatically reduced to 0.15 when the scene is mainly static (such as an empty corridor), thereby improving detection sensitivity.

[0402] In one embodiment, the determination of the adaptive threshold value of the current environment in S1214 can include:

[0403] S12141: Obtain the historical total feature quantity and the historical dynamic feature quantity of the current environment.

[0404] S12142: Determine the adaptive threshold value of the current environment according to the historical total feature quantity and the historical dynamic feature quantity.

[0405] In this embodiment, since the adaptive threshold value of the present application is strongly related to the current environment, when determining the adaptive threshold value of the current environment, the present application can first obtain the historical total feature quantity and the historical dynamic feature quantity of the current environment, and then determine the adaptive threshold value of the current environment according to the historical total feature quantity and the historical dynamic feature quantity. The specific formula is as follows:

[0406] ​​

[0407] in, This represents the number of historical dynamic features. The adaptive threshold for the current environment can be quickly determined using the formula described above.

[0408] In one embodiment, step S122, which uses an incremental spatiotemporal joint optimization engine to optimize the static feature set and the dynamic feature set to obtain decoupled target point cloud data and target image data, may include:

[0409] S1221: Generate a static map pose chain based on the static feature set, and determine the static residual corresponding to the static map pose chain.

[0410] S1222: Generate a dynamic object trajectory set based on the dynamic feature set, and determine the dynamic residual corresponding to the dynamic object trajectory set.

[0411] S1223: Determine the dynamic weighting factors of the current environment.

[0412] S1224: Construct a second objective function based on the static residual, the dynamic residual, and the dynamic weighting factor. After performing a sliding window incremental solution on the second objective function, determine the optimized static map pose chain and dynamic object trajectory set based on the second optimization result to obtain the decoupled target point cloud data and target image data.

[0413] In this embodiment, traditional methods typically treat dynamic objects as independent rigid bodies when decoupling feature points from static to dynamic states. While this reduces computational complexity, it leads to performance degradation due to oversimplification of motion, neglect of interaction, and incompatibility with deformation, thus affecting the accuracy of dynamic obstacle modeling. Therefore, this application utilizes an incremental spatiotemporal joint optimization engine to optimize both static and dynamic feature sets. This optimization is achieved through multi-sensor fusion pose graph construction, ensuring the consistency of the static environment. Based on kinematic constraints and multi-target tracking generation, accurate modeling of dynamic obstacles is realized, ultimately yielding decoupled target point cloud data and target image data.

[0414] Specifically, this application can first construct a static map pose chain corresponding to a static feature set by optimizing the pose graph of multi-sensor fusion, and determine the static residual corresponding to the static map pose chain. Then, based on kinematic constraints and multi-target tracking, a dynamic object trajectory set corresponding to a dynamic feature set is generated, and the dynamic residual corresponding to the dynamic object trajectory set is determined. Next, this application can also determine the dynamic weight factor of the current environment. In this way, a second objective function can be constructed based on the static residual, dynamic residual, and dynamic weight factor. After performing a sliding window incremental solution on the second objective function, the optimized static map pose chain and dynamic object trajectory set are determined based on the second optimization result to obtain the decoupled target point cloud data and target image data.

[0415] In one specific implementation, the optimization variables of this application include the static map pose chain. , For the first Camera pose of keyframes For keyframes index number, A rigid body motion group in three-dimensional space can represent the rotation and translation of a camera; a set of dynamic object trajectories. , Let be the trajectory of the m-th dynamic object, where m is the object's number. Let be the pose of the m-th dynamic object at time t. Given the time interval during which the m-th dynamic object appears within the field of view, the second objective function of this application is as follows:

[0416]

[0417] In the above formula, the first ∑ is the summation of observations of each static map point in the static feature set across all keyframes, with the initial value being the first keyframe of the first static map point. The second ∑ is the summation of observations of feature points on each dynamic object in the dynamic feature set at various times, with the initial value being the first time of the first feature point on the first dynamic object. The static residual of this application... , For the transformation from the camera to the world coordinate system, This is the intrinsic parameter matrix. For static map points, world coordinates For projection function, These are the pixel coordinates of point i on the static map as actually observed. The Mahalanobis distance, The covariance matrix of static observations and the dynamic residuals , Let be the pose of the dynamic object at time t. , Adjacent time intervals The Lie algebra of motion represents the time interval [time period]. The movement of objects within the space, where t is the current query time. Let k be the time interval. For the (k-1)th time interval, This represents the initial pose of the dynamic object. This is a chain multiplication operator, where n is the total number of time intervals and k is the index number of each time interval. This refers to mapping Lie algebras to Lie groups. Let j be the coordinates of a dynamic point j in the local coordinate system of the object. Let be the pixel coordinates of the dynamic point j observed at time t. For robust kernel functions, As a dynamic weighting factor, The covariance matrix is ​​for dynamic observations.

[0418] Once the second objective function is determined, this application can use a sliding window incremental approach to solve the second objective function. For example, when using a sliding window incremental approach to solve the function, this application can maintain only the optimization variables within the last 5 keyframes and eliminate old variables using the Schur complement method, thereby reducing computational complexity and optimizing solution efficiency. This improvement enables the algorithm to achieve millisecond-level response in dynamic environments, meeting the real-time standard requirements of robots.

[0419] In one embodiment, determining the dynamic weighting factor of the current environment in S1223 may include:

[0420] S12231: Obtain the number of historical static features and the number of historical dynamic features of the current environment.

[0421] S12232: Determine the dynamic weighting factor of the current environment based on the number of historical static features and the number of historical dynamic features.

[0422] In this embodiment, when determining the dynamic weight factor of the current environment, the number of historical static features and the number of historical dynamic features of the current environment can be obtained first. The dynamic weight factor of the current environment can then be determined based on these two numbers. The specific formula is as follows:

[0423]

[0424] This application can quickly determine the dynamic weighting factors of the current environment using the above formula, thereby effectively improving the accuracy of the optimization results.

[0425] In one embodiment, S120 involves locally updating the newly acquired global map based on the decoupled target point cloud data, target image data, and the target pose data to obtain a target map containing the inspection target. This update may include:

[0426] S123: Determine the static map pose chain in the decoupled target point cloud data and target image data.

[0427] S124: Based on the static map pose chain and the target pose data, locally update the latest acquired global map, determine the inspection target, and obtain a target map containing the inspection target.

[0428] In this embodiment, when locally updating the newly acquired global map based on the decoupled target point cloud data, target image data, and target pose data, a static map pose chain can first be determined in the decoupled target point cloud data and target image data. This static map pose chain is constructed through multi-sensor fusion pose graph optimization, ensuring the consistency of the static environment. Next, this application can locally update the newly acquired global map based on the static map pose chain and target pose data, and determine the inspection target to obtain a target map containing the inspection target.

[0429] Specifically, after decoupling the feature points in the target point cloud data and target image data, this application obtains a static map pose chain and a dynamic object trajectory set. At this point, the application can filter out the dynamic object trajectory set and use the static map pose chain to construct a static map. For example, this application can automatically filter out moving maintenance personnel and vehicle point clouds through static-dynamic decoupling. When the A-phase bushing thermometer is found to be obscured by a shadow, the shadow can be eliminated. Furthermore, changes in equipment status (such as the circuit breaker indicator light illuminating) can be marked as semantic features. Then, the static map pose chain is used to construct the corresponding static map, thereby improving the map's accuracy.

[0430] Furthermore, to improve real-time response performance and reduce computational complexity when constructing a static map, this application can also locally update the latest acquired global map based on the static map pose chain and target attitude data, thereby obtaining a target map containing the inspection target. For example, this application can add static equipment (current transformers, insulators) from the static map pose chain to the permanent map, and automatically associate the visually recognized equipment nameplate information (such as "main transformer B phase") with the point cloud cluster and the substation drawing coordinates; when a newly installed surge arrester is detected, a local area in the latest acquired global map can be updated; when encountering cable trench terrain, the point cloud height can be corrected using the target attitude data collected by the IMU, thereby further improving the accuracy of the target map.

[0431] In one embodiment, the local update of the latest acquired global map based on the static map pose chain and the target pose data in S124 may include:

[0432] S1241: Determine the set of environmental change quantification indicators corresponding to each feature point in the static map pose chain.

[0433] S1242: Determine the update frequency of the latest global map based on the set of environmental change quantification indicators.

[0434] S1243: The global map is locally updated based on the update frequency, the static map pose chain, and the target pose data.

[0435] In this embodiment, when locally updating the latest global map based on the static map pose chain and target pose data, an update frequency can be set to further improve the real-time performance of the system. This update frequency can be determined based on the set of environmental change quantification indicators corresponding to each feature point, thus reducing the amount of computation while meeting the real-time requirements.

[0436] In determining the set of environmental change quantification indicators corresponding to each feature point in the static map pose chain, this application can first determine the environmental change quantification indicators corresponding to each feature point, as detailed in the calculation process described above. Then, the average change quantification indicator of the static map pose chain is calculated, and this average change quantification indicator is used as the set of environmental change quantification indicators corresponding to each feature point. Finally, the update frequency of the latest global map is determined based on the environmental change quantification indicator set, as shown in the following formula:

[0437]

[0438] in, This refers to the update frequency of the global map. This application provides a set of quantitative indicators for environmental change. These indicators are used to dynamically adjust the map update frequency to balance real-time performance and map accuracy.

[0439] In one embodiment, S130, correcting the current positioning error based on the target point cloud data and the target image data to obtain a corrected positioning result, may include:

[0440] S131: Input the target point cloud data and the target image data into a pre-constructed spatiotemporal-semantic alignment model to obtain the alignment result output by the spatiotemporal-semantic alignment model.

[0441] S132: Correct the current positioning error based on the alignment result until the corrected positioning error is minimized, and obtain the corrected positioning result.

[0442] In this embodiment, when correcting the current positioning error based on target point cloud data and target image data, the pose of the lidar, the pose of the vision sensor, the laser-visual extrinsic parameters, and other auxiliary parameters can be determined using the target point cloud data and target image data. This allows for joint optimization, thereby reducing positioning errors. Furthermore, this application, through multimodal fusion positioning, can not only solve the lidar noise caused by reflective surfaces such as ceramic insulators, but also address the visual degradation problem under insufficient nighttime lighting.

[0443] For example, this application can locate the device outline and the position of the main transformer oil level gauge using lidar, verify the oil level scale reading and identify the nameplate fine-tuning position using a visual sensor. When the reflective surfaces such as porcelain insulators fail, visual recognition is the primary method. When visual recognition degrades due to insufficient nighttime lighting, laser positioning is the primary method. Furthermore, laser outline matching is preferred under strong light, and the IMU magnetometer is turned off in areas with electromagnetic interference to improve positioning accuracy.

[0444] Specifically, this application can pre-construct a spatiotemporal-semantic alignment model, which can be a multi-objective optimization function used in the vision-laser fusion system of robots or UAVs to jointly optimize feature matching, semantic consistency, and projection geometry error, thereby improving the localization and mapping accuracy in complex environments. Once the spatiotemporal-semantic alignment model is constructed, target point cloud data and target image data can be input into it to obtain the alignment result output by the model. Then, this application can correct the current localization error based on the alignment result until the corrected localization error is minimized, thus obtaining the final localization result. After generating the corresponding inspection map using this localization result, accurate navigation can be achieved.

[0445] In one embodiment, inputting the target point cloud data and the target image data into a pre-constructed spatiotemporal-semantic alignment model in S131 to obtain the alignment result output by the spatiotemporal-semantic alignment model may include:

[0446] S1311: Determine the state variables corresponding to the target point cloud data and the target image data, and construct a spatiotemporal-semantic union element.

[0447] S1312: Input the target point cloud data, the target image data, the state variables, and the spatiotemporal-semantic joint voxels into a pre-constructed spatiotemporal-semantic alignment model to obtain the alignment result output by the spatiotemporal-semantic alignment model.

[0448] In this embodiment, when using a pre-built spatiotemporal-semantic alignment model to optimize the localization error, the state variables corresponding to the target point cloud data and the target image data can be determined first, as follows:

[0449] The laser feature set corresponding to the target point cloud data in this application is:

[0450]

[0451] in, Let J be the laser feature set, which contains j laser features, each of which is a triplet. For the j-th laser feature Point cloud coordinates in three-dimensional space, For the j-th laser feature The laser descriptor is a 128-dimensional real vector. For the j-th laser feature The local surface covariance matrix is ​​usually a 3×3 symmetric positive definite matrix, which can be obtained by calculating the distribution of the neighborhood point cloud using PCA.

[0452] The visual feature set corresponding to the target image data in this application is:

[0453]

[0454] in, Let be a visual feature set, containing i visual features, each of which is a triplet. For the i-th visual feature pixel coordinates, For the i-th visual feature The visual descriptor is a 256-dimensional real vector. For the i-th visual feature The semantic categories consist of C classes, where C is the total number of semantic categories. These categories can be generated by a pre-trained semantic segmentation network and aligned with the COCO categories.

[0455] Next, this application can determine the state variables corresponding to the target point cloud data and the target image data, these state variables... It is expressed as follows:

[0456]

[0457] in, For global pose, For the Lie algebraic form of laser-vision extrinsics, For other auxiliary parameters, such as environmental feature descriptors, sensor internal states (e.g., IMU bias), or deep learning encoded features, It is a special European style group. For Lie algebra. When this application... When SGID (Semantic-Geometric Integrated Descriptor) is used as a network parameter, its network structure is as follows:

[0458]

[0459] in, For the i-th pair of image features and laser features, This provides spatial consistency information for geometric difference vectors. This is the inverse of the camera intrinsic matrix, used to back-project pixel coordinates onto the normalized camera coordinate system. It is a multilayer perceptron used to fuse visual, laser, and geometric difference information.

[0460] Next, this application can construct spatiotemporal-semantic union elements, as follows:

[0461]

[0462] in, A dynamic set of voxels, used to represent dynamic environments or dynamic objects. The k-th voxel, where k is the voxel index. Let the coordinates be the center coordinates of the voxels. For timestamp windows (ΔT=0.1s), The semantic probability distribution can be updated using Bayesian methods. The length of the timestamp window. The current time is as follows:

[0463]

[0464] in, This represents the semantic probability distribution after the nth update. This represents the semantic probability distribution (prior) for the (n-1)th update. For the observation term (likelihood), i.e., at location ,time Semantic categories observed The probability of the observation item The dynamic feature set obtained after the above decoupling of static and dynamic features can be used for initialization.

[0465] Furthermore, the spatiotemporal-semantic alignment model of this application can be:

[0466]

[0467] in, For the residuals of the visual factors, For describing sub-matching items, For semantic consistency items, For the projection geometry error term, This is a cross-modal descriptor mapping network (two-stream CNN + PointNet structure), with visual descriptors as input. Output and laser descriptor By mapping visual descriptors to the same feature space as laser descriptors, direct comparison can be facilitated. The distance matrix is ​​a Mahalanobis distance matrix, which can be updated through online learning. This represents a matching pair between the i-th visual feature and the j-th laser feature. For camera projection model, This is the intrinsic parameter matrix. Is with The semantic probability distribution of the corresponding modality (or the distribution from the prior / map). For laser-visual extrinsics, KL divergence measures the difference between two distributions. These are semantic weight coefficients, used to balance the importance of semantic terms. These are geometric weighting coefficients, which control the contribution of geometric errors.

[0468] When the target point cloud data, target image data, state variables, and spatiotemporal-semantic joint voxels are input into the above spatiotemporal-semantic alignment model, the alignment result output by the spatiotemporal-semantic alignment model can be obtained. By optimizing the alignment result, the present application achieves accurate alignment of multimodal data (visual, laser, semantic), thereby solving the limitation problem of a single sensor in complex environments.

[0469] In one embodiment, step S132, correcting the current positioning error based on the alignment result until the corrected positioning error is minimized, to obtain the corrected positioning result, may include:

[0470] S1321: Calculate the compensation amount of the state variable based on the state variable and the alignment result. After updating the state variable according to the compensation amount, return to execute the input of the target point cloud data, the target image data, the state variable and the spatiotemporal-semantic joint voxel into the pre-constructed spatiotemporal-semantic alignment model and its subsequent steps until the calculated compensation amount is optimal.

[0471] S1322: Use the state variable when the compensation amount is optimal as the corrected positioning result.

[0472] In this embodiment, after obtaining the alignment result, the joint optimization engine can be used to optimize the alignment result to achieve accurate alignment of multimodal data (visual, laser, semantic).

[0473] Specifically, this application can first calculate the compensation amount of the state variables based on the state variables and the alignment results, and then update the state variables based on the compensation amount. Next, this application can continue to input the target point cloud data, target image data, updated state variables, and spatiotemporal-semantic joint voxels into the pre-constructed spatiotemporal-semantic alignment model, and obtain the alignment result output by the spatiotemporal-semantic alignment model. Then, based on the updated state variables and the alignment result, the compensation amount of the updated state variables is calculated, and it is determined whether the compensation amount is the optimal compensation amount. If the compensation amount is the optimal compensation amount, the updated state variables are updated again using the compensation amount to obtain the corrected positioning result. If the compensation amount is not the optimal compensation amount, the state variables can be updated again based on the compensation amount, and the alignment result can be calculated again until the calculated compensation amount is optimal. In this way, accurate alignment of multimodal data (visual, laser, semantic) can be achieved, thereby reducing positioning errors.

[0474] In one embodiment, the alignment result may include residual values ​​of visual factors and residual values ​​of semantic constraint factors.

[0475] S1321, calculating the compensation amount of the state variable based on the state variable and the alignment result, may include:

[0476] S13211: Determine the residual value of the laser factor based on the target point cloud data.

[0477] S13212: The cross-modal residual is obtained by weighted summing of the residual values ​​of the visual factor, the semantic constraint factor, and the laser factor.

[0478] S13213: Calculate the compensation amount of the state variable based on the state variable and the cross-modal residual.

[0479] In this embodiment, when calculating the compensation amount of the state variable, the residual value of the laser factor can be determined first based on the target point cloud data. Then, the residual values ​​of the visual factor and the semantic constraint factor can be determined based on the alignment results. Next, the residual values ​​of the visual factor, the semantic constraint factor, and the laser factor are weighted and summed to obtain the cross-modal residual. Finally, this application can calculate the compensation amount of the state variable based on the state variable and the cross-modal residual.

[0480] Specifically, the residual value of the laser factor in this application can be obtained by the following formula:

[0481]

[0482] in, Is with The corresponding 3D coordinates of the global map point in the global coordinate system. Let be the covariance matrix (3×3) of the j-th laser point, describing the uncertainty of the point's location.

[0483] In this application, when determining the residual values ​​of the visual factors and the semantic constraint factors based on the alignment results, the residual values ​​of the visual factors can be those in the aforementioned spatiotemporal-semantic alignment model. The residual value of the semantic constraint factor can be from the spatiotemporal-semantic alignment model described above. Once the individual residual values ​​are obtained, they can be weighted and summed to obtain the cross-modal residual. Then, this application can calculate the compensation amount of the state variable based on the state variable and the cross-modal residual.

[0484] Furthermore, when calculating the compensation amount of the state variables based on the state variables and cross-modal residuals, this application can use a joint optimization method of multi-sensor fusion and semantic guidance. This method achieves high-precision incremental updates of pose and extrinsic parameters by decomposing the Jacobian matrix and dynamically adjusting the regularization parameters. Its core objective is to calculate the optimal increment of the state variables by minimizing the cross-modal residuals to compensate for the state variables.

[0485] For example, this application can first construct a joint Jacobian matrix:

[0486]

[0487] in, For the joint Jacobian matrix, For cross-modal residuals, For state variables, The linearization point of the current state (the state value used when differentiating).

[0488] Next, this application can decompose the joint Jacobian matrix into the following using the implicit differentiation chain rule:

[0489]

[0490] in, To align the derivatives of the residuals with respect to the state variables, Output the derivative of the SGID network with respect to the network parameter θ. To match the derivative of the residual with respect to the laser-visual extrinsic parameters in the laser point cloud, For external parameters to state variables The derivative of (usually the identity mapping or a simple transformation). The derivative of semantic constraints with respect to the descriptor. semantic variables to state variables The derivative of .

[0491] Furthermore, the compensation amount can be calculated using the following formula:

[0492]

[0493] in, This represents the compensation amount (increment) for the state variables, i.e., the update amount of the state variables in this iteration. For the sensor noise matrix, The values ​​represent the measurement noise variance for vision and laser, respectively, and the weighted reliability of different sensors. Let be the regularization matrix, where the extrinsic regularization term is . Based on semantic confidence Dynamic adjustment The coefficients of the pose regularization term are: For SGID network parameters, the regularization coefficients are... This is the inverse noise matrix, used to weight the residuals based on sensor reliability.

[0494] Parameter update after compensation:

[0495]

[0496] in, Let be the extrinsic parameter matrix of the laser radar to the camera at the t-th iteration. Let be the extrinsic parameter matrix after the (t+1)th iteration. For Lie algebra increments, This represents the global pose after the (t+1)th iteration update. Let be the global pose at the t-th iteration. For the increment of global pose, This refers to incremental update operations on a Lie group.

[0497] This application achieves breakthroughs in positioning accuracy, dynamic adaptability, and cross-modal consistency through a triple innovative mechanism of spatiotemporal-semantic joint voxels, multimodal factor graph optimization, and semantic-guided error compensation. Deep coupling with the dynamic environment SLAM module constructs a complete dynamic scene cognition system from low-level perception to high-level understanding, providing a new paradigm for multimodal fusion positioning in fields such as autonomous driving and embodied intelligence.

[0498] In one embodiment, the weighted summation of the residual values ​​of the visual factor, the semantic constraint factor, and the laser factor in step S13212 to obtain the cross-modal residual may include:

[0499] S32121: Determine the scene entropy and environmental change sensitivity of the current environment based on the target point cloud data and the target image data.

[0500] S32122: Determine the visual weight corresponding to the residual value of the visual factor, the semantic weight corresponding to the residual value of the semantic constraint factor, and the laser weight corresponding to the residual value of the laser factor based on the scene entropy and the environmental change sensitivity.

[0501] S32123: The cross-modal residual is obtained by weighted summation of the residual values ​​and corresponding visual weights of the visual factors, the residual values ​​and corresponding semantic weights of the semantic constraint factors, and the residual values ​​and corresponding laser weights of the laser factors.

[0502] In this embodiment, when performing weighted summation, the scene entropy and environmental change sensitivity of the current environment can be determined first based on the target point cloud data and target image data. Then, the visual weights corresponding to the residual values ​​of the visual factors, the semantic weights corresponding to the residual values ​​of the semantic constraint factors, and the laser weights corresponding to the residual values ​​of the laser factors can be determined based on the scene entropy and environmental change sensitivity. In this way, the cross-modal residuals can be obtained by performing weighted summation based on the residual values ​​of the visual factors and their corresponding visual weights, the residual values ​​of the semantic constraint factors and their corresponding semantic weights, and the residual values ​​of the laser factors and their corresponding laser weights.

[0503] Specifically, when determining the scene entropy and environmental change sensitivity of the current environment based on target point cloud data and target image data, this application can calculate the entropy of grayscale or gradient histograms for target image data by dividing the image into blocks, and then calculate the scene entropy after global averaging. For target point cloud data, this application can calculate the scene entropy based on the entropy values ​​of point density, normal changes, or semantic distribution. The environmental change sensitivity of this application can be determined through the calculation process of the above-mentioned environmental change quantification indicators.

[0504] Once the scene entropy and environmental change sensitivity of the current environment are determined, this application can calculate the visual weight corresponding to the residual value of the visual factor, the semantic weight corresponding to the residual value of the semantic constraint factor, and the laser weight corresponding to the residual value of the laser factor according to the following formulas:

[0505]

[0506]

[0507] in, For scene entropy, Let be the scene entropy at the current time t. Let be the scene entropy at the previous time t-1. The change in scene entropy is calculated as the difference between the Euclidean norm (2-norm) of the scene entropy at the current moment and the scene entropy at the previous moment. Sensitivity to environmental changes As weights, different weights can be obtained when different scene entropy and environmental change sensitivity are input into this application.

[0508] In one embodiment, such as Figure 4 As shown, Figure 4 This is a flowchart illustrating the generation of inspection paths and control commands provided in an embodiment of this application; the corrected positioning results may include semantic feature variables, semantic topological constraints, and dynamic obstacle probability distributions.

[0509] S130, which generates inspection paths and control commands based on the corrected positioning results and the target map, may include:

[0510] S133: Construct a semantic state set based on semantic feature variables and target map, map the semantic state set onto the semantic manifold, and generate an optimization target based on the mapping result.

[0511] S134: Constructing stochastic chance constraints based on the probability distribution of dynamic obstacles.

[0512] S135: Perform hierarchical optimization of the optimization objective based on semantic topological constraints and stochastic chance constraints, and determine the inspection path and control instructions based on the optimization results.

[0513] In this embodiment, after the current positioning error is corrected, an inspection path and control commands can be generated based on the corrected positioning result and the target map, and the inspection operation can be executed according to the inspection path and control commands. For example, the corrected positioning result may include the current global pose, laser-visual extrinsic parameters, environmental features, etc. Therefore, the application can generate obstacle avoidance paths (such as bypassing temporary fences) based on the relevant information in the positioning result and the target map. It can also optimize gait parameters for anti-slip grid ground, adjust the center of gravity height when crossing cable trenches, and stop and replan when encountering sudden obstacles (fallen tools), thereby reducing operational risks and improving autonomy and intelligence.

[0514] In one specific implementation, this application can first construct a semantic state set based on semantic feature variables and a target map, then map the semantic state set onto a semantic manifold, and generate an optimization objective based on the mapping result. Next, this application can also construct stochastic chance constraints based on the probability distribution of dynamic obstacles. The specific construction process can be set according to existing technologies and will not be elaborated here. Once the constraints and optimization objective are determined, this application can perform hierarchical optimization of the optimization objective based on semantic topological constraints and stochastic chance constraints, and determine the inspection path and control commands based on the optimization results.

[0515] Wherein, the semantic state set at time k in this application It can be defined as follows:

[0516]

[0517] in, Let be the position variable of the robot in three-dimensional space. For the robot's pose variables, The roll angle is the angle of rotation about the x-axis. The pitch angle is rotated about the y-axis. The yaw angle is the angle of rotation about the z-axis. Let the robot's velocity variables be the three coordinate axes. Let there be m semantic feature variables (such as predicted speed of dynamic obstacles, friction coefficient of ground material, etc.), where m is the total number of semantic variables. It has 9+m dimensions (the first 9 dimensions are geometric states, and the last m dimensions are semantic variables). The position, attitude, and velocity variables mentioned above can be predicted by combining the target map and semantic feature variables.

[0518] Since the aforementioned set of semantic states is a semantic representation of high-level objectives (such as "safe driving" and "optimal energy consumption"), this application can associate it with underlying physical quantities through manifold projection to determine the optimization objective. For example, if the automated inspection robot in this application wants to achieve the semantic objective of "emergency obstacle avoidance," it can map the corresponding set of semantic states "obstacle distance" and "vehicle speed" onto the semantic manifold to form a risk level, and take the minimum risk level as the optimization objective.

[0519] Furthermore, this application can also embed the set of semantic states into the dynamic model, as shown in the following formula:

[0520]

[0521] in, This is the state transition matrix, which contains physical parameters such as robot mass and moment of inertia. This is the system state vector at time k, which contains the robot's dynamic physical quantities (such as position, velocity, attitude angle, etc.). Let k+1 be the system state vector. To control the input matrix (corresponding to the joint torque of 6 degrees of freedom). This is the semantic coupling matrix, which describes the influence of semantic features on motion. The process noise covariance matrix is... The control input vector corresponds to the control commands of the actuator (such as joint torque and motor voltage). It is a set of semantic states.

[0522] Next, this application can perform hierarchical optimization of the optimization objective based on semantic topological constraints and stochastic chance constraints. The specific hierarchical objective function is as follows:

[0523]

[0524] in, To track performance items, To control the cost item, For semantic penalty terms, To predict the actual system output at step k in the time domain, The reference trajectory is the target trajectory or state sequence that the system is expected to track in the future time domain. The output weight matrix is ​​denoted as , and the control input is denoted as at step k. To control the input weight matrix, For semantic trade-off coefficients, For semantic penalty function, For semantic manifold mapping matrix, Let the semantic constraint space be the semantically valid region (e.g., "collision risk < threshold" or "energy efficiency > minimum standard"). The typical form of the semantic penalty function is as follows:

[0525]

[0526] in, It is a distance metric (such as Riemannian geometric distance) on a semantic manifold, used to evaluate the actual semantic state and the legal region. The degree of deviation, This refers to mapping the system output at step k to the semantic feature space. As a distance metric on the semantic manifold, it computes the mapped semantic state and the legal region. The degree of deviation, To predict the length of the time domain (optimize the number of steps).

[0527] In the above embodiments, this application reduces computational complexity by separating high-level global planning from low-level local control, and improves robustness by explicitly modeling environmental dynamics (such as moving obstacles) and sensor noise. Thus, during substation inspections, it can generate an initial path based on a semantic map (equipment distribution, safety level) and mark high-risk areas. It can also replan local trajectories after detecting dynamic obstacles (such as birds), and adjust foot force in real time to avoid instability on slippery insulators, while ensuring the robotic arm is aligned with the detection point.

[0528] The automatic inspection device in complex environments provided in the embodiments of this application will be described below. The automatic inspection device in complex environments described below and the automatic inspection method in complex environments described above can be referred to and correspond to each other.

[0529] In one embodiment, such as Figure 5 As shown, Figure 5 This application provides a schematic diagram of an automatic inspection device in a complex environment, as illustrated in an embodiment of the present application. The present application also provides an automatic inspection device in a complex environment, which may include a data acquisition module 210, a map generation module 220, and an automatic inspection module 230, as detailed below:

[0530] The data acquisition module 210 is used to acquire target point cloud data, target image data and target attitude data collected by multiple pre-integrated sensors at the same time and in the same space during inspection.

[0531] The map generation module 220 is used to decouple the feature points in the target point cloud data and the target image data from static and dynamic states, and to locally update the latest global map based on the decoupled target point cloud data, target image data and target attitude data to obtain a target map containing the inspection target.

[0532] The automatic inspection module 230 is used to correct the current positioning error based on the target point cloud data and the target image data, and generate an inspection path and control instructions based on the corrected positioning results and the target map, and execute the inspection operation according to the inspection path and the control instructions.

[0533] In the above embodiments, during inspection, target point cloud data, target image data, and target attitude data collected simultaneously by multiple pre-integrated sensors in the same time and space can be acquired first. This allows for spatiotemporal synchronization of data collected by different sensors, ensuring the geometric and physical consistency of the motion trajectory, thereby improving dynamic positioning accuracy. Furthermore, even if some sensors fail, the system can still maintain functionality through synchronized redundant data, effectively enhancing system robustness. Spatiotemporal synchronization can also reduce data processing latency, thus meeting the real-time control requirements of dynamic scenes. Next, this application can decouple the feature points in the target point cloud data and target image data from static and dynamic states. This not only allows for the identification and filtering of dynamic features, but also... Furthermore, the newly acquired global map can be locally updated based on the static features in the decoupled target point cloud data, target image data, and target attitude data. This reduces computational load while updating the map in real time according to the current environment, thereby improving navigation and positioning accuracy and adapting to complex environments. After obtaining the target map containing the inspection target, this application can also correct the current positioning error based on the target point cloud data and target image data to further improve positioning accuracy. Finally, this application can generate inspection paths and control commands based on the corrected positioning results and target map, and execute inspection operations according to the inspection paths and control commands, thereby achieving automatic inspection while effectively reducing operational risks.

[0534] In one embodiment, this application also provides a computer-readable storage medium storing computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the steps of the automatic inspection method in a complex environment as described in any of the above embodiments.

[0535] In one embodiment, this application also provides a computer device, including: one or more processors, and memory.

[0536] The memory stores computer-readable instructions, which, when executed by the one or more processors, perform the steps of the automatic inspection method in a complex environment as described in any of the above embodiments.

[0537] Indicatively, such as Figure 6 As shown, Figure 6 This is a schematic diagram of the internal structure of a computer device 300 provided in an embodiment of this application. The computer device 300 can be provided as a server. (Refer to...) Figure 6The computer device 300 includes a processing component 302, which further includes one or more processors, and memory resources represented by memory 301 for storing instructions, such as application programs, that can be executed by the processing component 302. The application programs stored in memory 301 may include one or more modules, each corresponding to a set of instructions. Furthermore, the processing component 302 is configured to execute instructions to perform the automated inspection method in a complex environment according to any of the above embodiments.

[0538] The computer device 300 may also include a power supply component 303 configured to perform power management of the computer device 300, a wired or wireless network interface 304 configured to connect the computer device 300 to a network, and an input / output (I / O) interface 305. The computer device 300 may operate on an operating system stored in memory 301, such as Windows Server™, Mac OS X™, Unix™, Linux™, Free BSD™, or similar.

[0539] Those skilled in the art will understand that Figure 6 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0540] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0541] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. The various embodiments can be combined as needed, and the same or similar parts can be referred to each other.

[0542] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An automatic inspection method for complex environments, characterized in that, The method includes: During inspection, target point cloud data, target image data, and target attitude data are acquired simultaneously from multiple pre-integrated sensors in the same space. The feature points in the target point cloud data and the target image data are decoupled from static and dynamic states. Based on the decoupled target point cloud data, target image data and target pose data, the latest acquired global map is locally updated to obtain a target map containing the inspection target. The current positioning error is corrected based on the target point cloud data and the target image data. An inspection path and control instructions are generated based on the corrected positioning results and the target map. The inspection operation is then executed according to the inspection path and the control instructions.

2. The automatic inspection method in complex environments according to claim 1, characterized in that, Each of the aforementioned sensors includes at least a lidar, a vision sensor, and an inertial measurement unit; The acquisition of target point cloud data, target image data, and target pose data collected simultaneously by multiple pre-integrated sensors in the same time and space includes: A global clock signal is determined, and the lidar, the vision sensor, and the inertial measurement unit are triggered by the global clock signal to synchronously acquire raw point cloud data, target image data, and raw attitude data. After aligning the original pose data with the timestamps of the original point cloud data or the target image data, the target pose data is obtained. Obtain the coordinate transformation matrix between the lidar and the vision sensor, and spatially align the original point cloud data with the target image data according to the coordinate transformation matrix to obtain the target point cloud data.

3. The automatic inspection method in complex environments according to claim 2, characterized in that, The determination of the global clock signal includes: A unified timestamp after multi-sensor synchronization is generated by a pre-configured time synchronization model, and a global clock signal is generated based on the unified timestamp. Alternatively, after generating a unified clock signal through a specific chip, the unified clock signal is input to a pre-configured time synchronization model, and a unified timestamp after multi-sensor synchronization is generated through the time synchronization model. A global clock signal is then generated based on the unified timestamp.

4. The automatic inspection method in complex environments according to claim 3, characterized in that, The process of generating a unified timestamp after multi-sensor synchronization using a pre-configured time synchronization model includes: The absolute time for the lidar to complete scanning of a frame of point cloud, the calibration time delay between the lidar and the vision sensor, and the three-dimensional angular velocity vector and angular velocity compensation coefficient measured by the inertial measurement unit are determined. The absolute time, the calibration time delay, the three-dimensional angular velocity vector, and the angular velocity compensation coefficient are input into a pre-configured time synchronization model, and a unified timestamp after multi-sensor synchronization is generated through the time synchronization model.

5. The automatic inspection method in complex environments according to claim 2, characterized in that, The step of aligning the original pose data with the timestamps of the original point cloud data or the target image data to obtain the target pose data includes: Determine the timestamps corresponding to each visual keyframe in the original point cloud data or the target image data, and interpolate the original pose data according to each timestamp to obtain virtual pose data at each timestamp. Based on the time distribution of each timestamp, multiple sub-intervals and virtual pose data for each sub-interval are determined. After pre-integrating the virtual pose data of each sub-interval, the pre-integrated value of each sub-interval is obtained. The virtual attitude data of each sub-interval is corrected by using the pre-integral values ​​of each sub-interval to obtain the target attitude data.

6. The automatic inspection method in complex environments according to claim 5, characterized in that, When the original attitude data meets the normal interpolation conditions, the step of interpolating the original attitude data according to each timestamp to obtain the virtual attitude data at each timestamp includes: Obtain the quaternion corresponding to each adjacent time point in the original attitude data; Quaternion spherical linear interpolation is used to interpolate between quaternions corresponding to adjacent timestamps at each timetamp to obtain virtual attitude data at each timetamp.

7. The automatic inspection method in complex environments according to claim 5, characterized in that, When the original attitude data meets the emergency interpolation conditions, the interpolation of the original attitude data based on each timestamp to obtain virtual attitude data at each timestamp includes: The original attitude data is interpolated at each time stamp using a preset emergency interpolation mode to obtain virtual attitude data at each time stamp.

8. The automatic inspection method in complex environments according to claim 5, characterized in that, The pre-integration of the virtual pose data for each sub-interval yields the pre-integrated value for each sub-interval, including: The virtual pose data of each sub-interval is transformed into the keyframe coordinate system to obtain the transformed virtual pose data; Pre-integrate the transformed virtual attitude data in each sub-interval to obtain the pre-integrated value for each sub-interval.

9. The automatic inspection method in complex environments according to claim 6, characterized in that, Before correcting the virtual pose data of each sub-interval using the pre-integral values ​​of each sub-interval, the method further includes: When the virtual pose data satisfies the Lie group-Lie algebra optimization condition, Lie algebra optimization is performed on the virtual pose data of each sub-interval to make the interpolation path conform to the maximum steering angle and acceleration limit, and the interpolation parameters are dynamically adjusted according to the semantic information of each visual keyframe to obtain the optimized virtual pose data.

10. The automatic inspection method in complex environments according to claim 9, characterized in that, The virtual pose data for each sub-interval is optimized using Lie algebra to ensure the interpolation path conforms to the maximum steering angle and acceleration limits. The interpolation parameters are then dynamically adjusted based on the semantic information of each visual keyframe to obtain the optimized virtual pose data, including: The virtual pose data of each sub-interval is interpolated using the Lie algebra space linear interpolation method. During the interpolation process, the interpolation weights are dynamically adjusted according to the semantic information of each visual keyframe. The virtual pose data is optimized using a pre-integration-Kalman joint filtering architecture. The interpolation path is adjusted using an acceleration-aware Bézier spline path. Finally, an asymmetric noise suppression strategy is used to process the noise in each virtual pose data in frequency bands, resulting in optimized virtual pose data.

11. The automatic inspection method in complex environments according to claim 9, characterized in that, The step of correcting the virtual attitude data of each sub-interval using the pre-integral values ​​of each sub-interval to obtain the target attitude data includes: Based on the pre-integral quantities of each sub-interval, a first objective function is constructed with the goal of minimizing the overall residual during multi-sensor fusion. The first optimization result is obtained by optimizing and solving the first objective function. Based on the first optimization result, the virtual pose data of each sub-interval is corrected to obtain the target pose data.

12. The automatic inspection method in complex environments according to any one of claims 1-11, characterized in that, The step of decoupling the feature points in the target point cloud data and the target image data to obtain decoupled target point cloud data and target image data includes: Determine the dynamic probability value of each feature point in the target point cloud data and the target image data, and divide each feature point into a static feature set and a dynamic feature set according to the dynamic probability value of each feature point; The static feature set and the dynamic feature set are optimized using an incremental spatiotemporal joint optimization engine to obtain decoupled target point cloud data and target image data.

13. The automatic inspection method in complex environments according to claim 12, characterized in that, Determining the dynamic probability value of each feature point in the target point cloud data and the target image data includes: For each feature point in the target point cloud data and the target image data: Obtain the 3D coordinates, feature descriptor, and timestamp of the feature point, as well as the geometric and semantic weights of the current environment; The geometric factors of the feature point are determined based on its 3D coordinates, feature descriptor, and timestamp, and the semantic factors of the feature point are determined based on its feature descriptor. The dynamic probability value of a feature point is determined based on its geometric factors, geometric weights, semantic factors, and semantic weights.

14. The automatic inspection method in complex environments according to claim 13, characterized in that, The process of obtaining the geometric weights and semantic weights of the current environment includes: Obtain the total number of historical features and the number of historical static features of the current environment; The geometric and semantic weights of the current environment are determined based on the total number of historical features and the number of historical static features.

15. The automatic inspection method in complex environments according to claim 13, characterized in that, Determining the semantic factor of a feature point based on its feature descriptor includes: Determine whether the feature point satisfies the semantic factor constraint based on the feature descriptor of the feature point; If satisfied, the semantic factor of the feature point is determined according to the preset entropy range.

16. The automatic inspection method in complex environments according to claim 13, characterized in that, The step of determining the dynamic probability value of a feature point based on its geometric factors, geometric weights, semantic factors, and semantic weights includes: Based on the 3D coordinates, feature descriptor, and timestamp of the feature point, determine whether the feature point meets the reliability assessment conditions; If satisfied, the depth confidence and environmental change quantification index of the feature point are determined, and the dynamic probability value of the feature point is determined based on the geometric factor, geometric weight, semantic factor, semantic weight, depth confidence and environmental change quantification index of the feature point.

17. The automatic inspection method in complex environments according to claim 16, characterized in that, The quantitative indicators for determining the environmental changes at the feature point include: Determine the voxel corresponding to the feature point, the voxel gradient, the voxel's occupancy state at the previous time step, the voxel's occupancy state at the current time step, and all sensor observation data from the initial time step to the current time step; Based on the voxel corresponding to the feature point, the voxel gradient, the voxel's occupancy state at the previous time step, the voxel's occupancy state at the current time step, and all sensor observation data from the initial time step to the current time step, calculate the environmental change quantification index of the feature point.

18. The automatic inspection method in complex environments according to claim 12, characterized in that, The step of dividing each feature point into a static feature set and a dynamic feature set based on the dynamic probability value of each feature point includes: Determine the adaptive threshold for the current environment; Based on the dynamic probability value of each feature point and the adaptive threshold, each feature point is divided into a static feature set and a dynamic feature set.

19. The automatic inspection method in complex environments according to claim 18, characterized in that, The determination of the adaptive threshold for the current environment includes: Obtain the total number of historical features and the number of historical dynamic features of the current environment; The adaptive threshold for the current environment is determined based on the total number of historical features and the number of historical dynamic features.

20. The automatic inspection method in complex environments according to claim 12, characterized in that, The incremental spatiotemporal joint optimization engine is used to optimize the static feature set and the dynamic feature set to obtain decoupled target point cloud data and target image data, including: A static map pose chain is generated based on the static feature set, and the static residual corresponding to the static map pose chain is determined. A dynamic object trajectory set is generated based on the dynamic feature set, and the dynamic residual corresponding to the dynamic object trajectory set is determined. Determine the dynamic weighting factors for the current environment; A second objective function is constructed based on the static residual, the dynamic residual, and the dynamic weighting factor. After solving the second objective function using a sliding window incremental method, the optimized static map pose chain and dynamic object trajectory set are determined based on the second optimization result to obtain the decoupled target point cloud data and target image data.

21. The automatic inspection method in complex environments according to claim 20, characterized in that, The determination of the dynamic weighting factors for the current environment includes: Obtain the number of historical static features and the number of historical dynamic features of the current environment; The dynamic weighting factor of the current environment is determined based on the number of historical static features and the number of historical dynamic features.

22. The automatic inspection method in complex environments according to any one of claims 1-11 and 13-21, characterized in that, The step of locally updating the latest acquired global map based on the decoupled target point cloud data, target image data, and target pose data to obtain a target map containing the inspection target includes: Determine the static map pose chain in the decoupled target point cloud data and target image data; The latest global map is locally updated based on the static map pose chain and the target pose data, and the inspection target is determined to obtain a target map containing the inspection target.

23. The automatic inspection method in complex environments according to claim 22, characterized in that, The step of locally updating the latest global map based on the static map pose chain and the target pose data includes: Determine the set of quantitative indicators of environmental change corresponding to each feature point in the static map pose chain; The update frequency of the latest global map is determined based on the set of quantitative indicators of environmental change. The global map is locally updated based on the update frequency, the static map pose chain, and the target pose data.

24. The automatic inspection method in complex environments according to any one of claims 1-11, 13-21, and 23, characterized in that, The step of correcting the current positioning error based on the target point cloud data and the target image data to obtain the corrected positioning result includes: The target point cloud data and the target image data are input into a pre-constructed spatiotemporal-semantic alignment model to obtain the alignment result output by the spatiotemporal-semantic alignment model; The current positioning error is corrected based on the alignment result until the corrected positioning error is minimized, thus obtaining the corrected positioning result.

25. The automatic inspection method in complex environments according to claim 24, characterized in that, The step of inputting the target point cloud data and the target image data into a pre-constructed spatiotemporal-semantic alignment model to obtain the alignment result output by the spatiotemporal-semantic alignment model includes: Determine the state variables corresponding to the target point cloud data and the target image data, and construct a spatiotemporal-semantic joint element; The target point cloud data, the target image data, the state variables, and the spatiotemporal-semantic joint voxels are input into a pre-constructed spatiotemporal-semantic alignment model to obtain the alignment result output by the spatiotemporal-semantic alignment model.

26. The automatic inspection method in complex environments according to claim 25, characterized in that, The step of correcting the current positioning error based on the alignment result until the corrected positioning error is minimized, to obtain the corrected positioning result, includes: Based on the state variable and the alignment result, calculate the compensation amount of the state variable. After updating the state variable according to the compensation amount, return to execute the input of the target point cloud data, the target image data, the state variable and the spatiotemporal-semantic joint voxel into the pre-constructed spatiotemporal-semantic alignment model and its subsequent steps until the calculated compensation amount is optimal. The state variable at which the compensation amount is optimal is used as the corrected positioning result.

27. The automatic inspection method in complex environments according to claim 26, characterized in that, The alignment result includes the residual values ​​of the visual factors and the residual values ​​of the semantic constraint factors; The calculation of the compensation amount for the state variables based on the state variables and the alignment result includes: The residual value of the laser factor is determined based on the target point cloud data; The cross-modal residual is obtained by weighted summing of the residual values ​​of the visual factor, the semantic constraint factor, and the laser factor. The compensation amount of the state variable is calculated based on the state variable and the cross-modal residual.

28. The automatic inspection method in complex environments according to claim 27, characterized in that, The step of weighted summing of the residual values ​​of the visual factor, the semantic constraint factor, and the laser factor to obtain the cross-modal residual includes: The scene entropy and environmental change sensitivity of the current environment are determined based on the target point cloud data and the target image data. Based on the scene entropy and the sensitivity to environmental changes, determine the visual weight corresponding to the residual value of the visual factor, the semantic weight corresponding to the residual value of the semantic constraint factor, and the laser weight corresponding to the residual value of the laser factor. The cross-modal residual is obtained by weighted summation of the residual values ​​and corresponding visual weights of the visual factors, the residual values ​​and corresponding semantic weights of the semantic constraint factors, and the residual values ​​and corresponding laser weights of the laser factors.

29. The automatic inspection method in complex environments according to any one of claims 1-11, 13-21, 23, and 25-28, characterized in that, The corrected localization results include semantic feature variables, semantic topological constraints, and dynamic obstacle probability distribution; The generation of inspection paths and control commands based on the corrected positioning results and the target map includes: A semantic state set is constructed based on the semantic feature variables and the target map, the semantic state set is mapped onto a semantic manifold, and an optimization target is generated based on the mapping result; A stochastic chance constraint is constructed based on the probability distribution of the dynamic obstacle; The optimization objective is optimized hierarchically based on the semantic topology constraints and the random chance constraints, and the inspection path and control instructions are determined based on the optimization results.

30. An automatic inspection device for complex environments, characterized in that, include: The data acquisition module is used to acquire target point cloud data, target image data, and target attitude data collected by multiple pre-integrated sensors at the same time and in the same space during inspection. The map generation module is used to decouple the feature points in the target point cloud data and the target image data from static and dynamic states, and to locally update the latest global map based on the decoupled target point cloud data, target image data and target pose data to obtain a target map containing the inspection target. The automatic inspection module is used to correct the current positioning error based on the target point cloud data and the target image data, and generate an inspection path and control instructions based on the corrected positioning results and the target map, and execute the inspection operation according to the inspection path and the control instructions.

31. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the steps of the automatic inspection method in a complex environment as described in any one of claims 1 to 29.

32. A computer device, characterized in that, include: One or more processors, and memory; The memory stores computer-readable instructions, which, when executed by the one or more processors, perform the steps of the automatic inspection method in a complex environment as described in any one of claims 1 to 29.

Images