Three-dimensional measurement data fusion device and method based on Beidou positioning and laser scanning

Abstract

The invention relates to the technical field of three-dimensional measurement and data processing, and discloses a three-dimensional measurement data fusion device and method based on Beidou positioning and laser scanning. The device comprises a Beidou positioning module, an inertial measurement unit, a laser scanning module comprising a holder scanner, a data fusion processing module, a data transmission module and a data storage and visualization module. The data fusion processing module estimates a state vector of the device and a state covariance matrix representing the uncertainty of the state vector based on a kinematic model; and when the uncertainty of the state vector exceeds a preset threshold value, the module actively generates a control instruction to drive the pan-tilt scanner to execute target scanning to obtain high-value observation data. The obtained high-value observation data passes through an observation noise covariance matrix and is used for updating a state vector, so that closed-loop control capable of rapidly converging uncertainty is formed. According to the invention, the fusion precision, the long-term stability and the reliability of a measurement system are improved through adaptive weighting and an on-line external parameter calibration technology.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of three-dimensional measurement and data processing, in particular to a three-dimensional measurement data fusion device and method based on Beidou positioning and laser scanning. BACKGROUND

[0002] In the field of large-scale ports, mines and automation engineering, high-precision real-time three-dimensional positioning and attitude measurement of large mobile equipment, and synchronous acquisition of three-dimensional geometric information of its working environment, are the key technical prerequisites for realizing automated and intelligent operation processes. At present, combining Global Navigation Satellite System (GNSS) (such as China's Beidou satellite navigation system) and laser scanning technology has become the main technical approach to realize such three-dimensional measurement. Generally, GNSS is used to provide absolute position information of large mobile equipment in the global coordinate system, and laser scanners are used to obtain high-density three-dimensional point cloud data of the environment around the large mobile equipment. By fusing the two heterogeneous data sources, theoretically, a comprehensive measurement result with global accuracy and local details can be obtained.

[0003] However, in practical applications, the existing technical solutions still have deficiencies in precision, reliability and environmental adaptability; on the one hand, the existing data fusion methods are mostly passive open-loop processing modes; such methods usually fuse the input data according to the preset fixed model and algorithm, lacking dynamic evaluation and feedback mechanism for real-time quality of sensor data and uncertainty of the fusion system itself state; when the Beidou positioning module has precision decline due to satellite signal obstruction, or the laser scanning module produces noise due to interference of environmental factors such as on-site dust, rain and fog, the passive fusion system cannot adaptively adjust the weight of different data sources in the fusion process, resulting in significant impact of low-quality data on the overall measurement precision and reduced reliability; on the other hand, the long-term stability of multi-sensor combined measurement system also faces challenges. The relative pose relationship between sensors installed on large equipment, i.e. the external parameter, will change slowly due to factors such as vibration, structure thermal expansion and contraction caused by long-time operation of large mobile equipment. This drift of external parameters will introduce systematic errors that are difficult to detect, and traditional offline calibration methods not only have complicated operation and affect the normal operation of large mobile equipment, but also cannot compensate for dynamic deviations generated during operation in real time. More importantly, when the positioning or orientation accuracy of the system decreases due to the above reasons, existing technologies lack the ability to actively restore accuracy; their laser scanning strategies are usually fixed and cannot intelligently adjust the scanning task according to the type of observation information that the system currently needs most; the system cannot actively focus scanning resources on stable environmental features that can most effectively constrain the direction of current uncertainty growth, and can only passively accept the reduction in measurement accuracy, rather than forming a closed-loop control that discovers, actively senses, and solves problems. This limits its application in automated operation scenarios that require continuous high reliability.

[0004] Therefore, this invention proposes a three-dimensional measurement data fusion device and method based on BeiDou positioning and laser scanning to address the shortcomings of existing technologies. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a three-dimensional measurement data fusion device and method based on BeiDou positioning and laser scanning, which solves the problem in three-dimensional measurement technology where the positioning and orientation accuracy decreases due to fluctuations in sensor data quality or lack of environmental features, and the inability to actively compensate for this.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning, the three-dimensional measurement data fusion device comprising: The BeiDou positioning module is used to obtain global positioning information of mobile equipment in the global coordinate system; An inertial measurement unit is used to acquire the three-dimensional angular velocity and three-dimensional acceleration information of the mobile equipment; A laser scanning module is used to acquire three-dimensional point cloud information of the environment surrounding the mobile equipment, and the laser scanning module includes multiple laser scanners, including long-range scanners and multi-layer scanners. The data fusion processing module is used to receive the global positioning information, three-dimensional angular velocity and three-dimensional acceleration information, and three-dimensional point cloud information. Based on the kinematic model, it estimates the state vector of the mobile equipment and the state covariance matrix representing the uncertainty of the state vector by fusing the global positioning information, the three-dimensional angular velocity and three-dimensional acceleration information, and the three-dimensional point cloud information. It monitors the state covariance matrix in real time. When the uncertainty of the state vector exceeds a preset uncertainty threshold, it generates control instructions for driving the long-range scanner to perform scanning of a specific area. The data transmission module is used to send the control command from the data fusion processing module to the laser scanning module to drive the laser scanner to perform targeted scanning of stable environmental features, thereby acquiring observation data; The data storage and visualization module is used to store the state vector and update the state vector using the observation data. During the update process, an observation noise covariance matrix is ​​set for the observation data to accelerate the convergence of the uncertainty.

[0007] This invention also provides a method for fusing three-dimensional measurement data based on BeiDou positioning and laser scanning, the method comprising the following steps: S1. Collect global positioning information of the mobile equipment through the Beidou positioning module, and collect three-dimensional angular velocity and three-dimensional acceleration information of the mobile equipment through the inertial measurement unit; S2. Collect three-dimensional point cloud information of the environment surrounding the mobile equipment through the laser scanning modules of multiple laser scanners, including long-range scanners and multi-layer scanners. S3. Based on the kinematic model, and by integrating the global positioning information, three-dimensional angular velocity and three-dimensional acceleration information, as well as the three-dimensional point cloud information, the state vector of the mobile equipment and the state covariance matrix representing the uncertainty of the state vector are estimated. S4. Monitor the state covariance matrix in real time. When the uncertainty of the state vector exceeds a preset uncertainty threshold, generate control instructions to drive the laser scanner to perform scanning of a specific area. S5. Send the control command to the laser scanning module to drive the laser scanner to perform targeted scanning of stable environmental features, thereby acquiring observation data; S6. Update the state vector using the observation data, wherein, during the update process, an observation noise covariance matrix is ​​set for the observation data to accelerate the convergence of the uncertainty.

[0008] This invention provides a device and method for fusing three-dimensional measurement data based on BeiDou positioning and laser scanning, which has the following beneficial effects: The device of this invention monitors the state covariance matrix representing state uncertainty in real time through a data fusion processing module. When the uncertainty exceeds a preset uncertainty threshold, it can actively generate control commands to drive the gimbal scanner to perform targeted scanning on the stable environmental features that can most effectively constrain the uncertainty. This closed-loop control mechanism ensures that the device can quickly converge uncertainty by actively acquiring high-value observation data in challenging scenarios where the quality of some sensor data deteriorates or effective environmental features are sparse, thereby ensuring the continuous high accuracy of the output state vector.

[0009] This invention utilizes an adaptive fusion algorithm unit that, during the update phase of the extended Kalman filter, does not employ fixed noise parameters but dynamically adjusts the observation noise covariance matrix. This adjustment is based on the satellite signal-to-noise ratio of BeiDou positioning information, the goodness of fit of laser point cloud registration, and the sensor health index output by the online calibration and monitoring unit. Through this adaptive weighting method, the device can quantify and utilize the data quality of each observation, automatically suppressing the contamination of the fusion result by low-quality data, thereby outputting a more accurate and reliable state vector.

[0010] The online calibration and monitoring unit of this invention can calculate and update the external parameters between the scanners in the laser scanning module in real time, and automatically compensate for the installation posture drift caused by vibration of moving equipment, structural deformation or temperature change. This online self-calibration capability without interruption of operation and without manual intervention avoids the accumulation of systematic measurement errors caused by inaccurate sensor external parameters, and ensures the data consistency and accuracy reliability of the three-dimensional measurement data fusion device throughout its entire life cycle. Attached Figure Description

[0011] Figure 1 This is a schematic diagram of the overall architecture of the three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the method flow according to an embodiment of the present invention.

[0012] Icon labels: 10. Beidou positioning module; 20. Laser scanning module; 30. Data fusion processing module; 40. Data transmission module; 50. Data storage and visualization module. Detailed Implementation

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

[0014] Figure 1 This is a schematic diagram of the overall architecture according to an embodiment of the present invention. Figure 1 As shown, this embodiment of the invention provides a three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning. The device may include: a BeiDou positioning module 10, a laser scanning module 20, a data fusion processing module 30, a data transmission module 40, and a data storage and visualization module 50.

[0015] The Beidou positioning module 10 and the laser scanning module 20 serve as data acquisition sources, respectively acquiring the global positioning information of the mobile equipment and the three-dimensional point cloud information of the surrounding environment. These two modules send the acquired raw data to the data fusion processing module 30 through the data transmission module 40. The data fusion processing module 30 processes and fuses the received data, outputting high-precision mobile equipment status and global point cloud data, and sends it to the data storage and visualization module 50 through the data transmission module 40 for storage and subsequent upper-level functional applications, such as realizing automatic calculation of material pile volume and mobile equipment anti-collision warning. At the same time, the data fusion processing module 30 can generate control commands based on the uncertainty of the fusion status, and send them back to the laser scanning module 20 through the data transmission module 40 to actively control the long-range scanner in it to perform targeted scanning of specific areas, thereby realizing closed-loop correction of status estimation.

[0016] In a preferred embodiment, the BeiDou positioning module 10 is used to acquire high-precision real-time position, speed, and time information of mobile equipment in a global coordinate system. Specifically, the BeiDou positioning module 10 includes at least one BeiDou high-precision positioning terminal supporting Real-Time-Kinematic (RTK) technology, a high-gain antenna, and a positioning data preprocessing unit. The high-gain antenna is installed in a location where the mobile equipment body is not easily obstructed, such as the top of the gantry leg or the root of the cantilever structure of large port machinery, to ensure good satellite signal reception. The positioning data preprocessing unit is used to perform preliminary processing on the raw data output by the positioning terminal, filtering out unreliable positioning data (i.e., gross errors) caused by signal interference or obstruction based on quality indicators such as the signal-to-noise ratio of satellite signals, thereby obtaining a cleaner positioning data sequence without significant outliers, and providing stable and reliable global positioning observations to the data fusion processing module 30.

[0017] The laser scanning module 20 is used to acquire high-density, high-precision three-dimensional point cloud data of the environment surrounding the mobile equipment. The laser scanning module 20 includes at least two laser scanners and a point cloud data preprocessing industrial control computer. In a specific embodiment, these laser scanners can be fixedly installed on both sides of the cantilever structure of the mobile equipment, or integrated on a high-precision two-dimensional gimbal and installed on the top of the mobile equipment to collect fine local point clouds of the working area. It can also receive control commands from the data fusion processing module 30 to achieve targeted scanning of specific areas. The point cloud data preprocessing industrial control computer is used to perform noise reduction and downsampling processing on the raw point cloud data collected by each scanner. For example, a statistical filtering algorithm is used to remove noise points caused by suspended particulate matter in the environment, and a voxel grid downsampling algorithm is used to reduce the amount of data while ensuring the structural characteristics of the point cloud.

[0018] The data fusion processing module 30 integrates multiple functional units, including: an online calibration and monitoring unit, a spatiotemporal registration unit, an adaptive fusion algorithm unit, and a scanning strategy generation unit. The online calibration and monitoring unit is used to calculate the relative installation positions and attitude relationships (i.e., extrinsic parameters) between the scanners in the laser scanning module 20 in real time, and to monitor the stability of these relative positional relationships (extrinsic parameters). The spatiotemporal registration unit is used to accurately register data from different sensors to the same spatial coordinate system based on a unified time reference and using the real-time extrinsic parameters output by the online calibration and monitoring unit. The adaptive fusion algorithm unit is used to execute the extended Kalman filter algorithm, fuse BeiDou positioning data and LiDAR data, and estimate the optimal state vector of the mobile equipment, which includes three-dimensional position, velocity, and attitude. The scanning strategy generation unit is used to monitor the state uncertainty output by the adaptive fusion algorithm unit in real time. When the uncertainty exceeds a preset uncertainty threshold, the scanning strategy generation unit actively generates control commands to instruct the long-range scanner in the laser scanning module 20 to perform a specific scanning task.

[0019] The data transmission module 40 is used to realize reliable data exchange between various hardware modules inside the device. In a preferred embodiment, the data transmission module 40 adopts a redundant communication architecture that combines optical fiber and industrial wireless AP. By deploying an industrial switch that supports high deterministic network protocols, it is ensured that when the primary link (optical fiber) fails, it can seamlessly switch to the backup link (industrial wireless), thereby ensuring low-latency and high-reliability transmission of large-capacity point cloud data streams and real-time control commands.

[0020] The data storage and visualization module 50 is the human-computer interaction and data archiving terminal of the device. This module specifically includes a data storage server and a three-dimensional visualization platform. The data storage server is used for long-term archiving of the raw data collected by the Beidou positioning module 10 and the laser scanning module 20, as well as the final fusion result output by the data fusion processing module 30. The three-dimensional visualization platform loads a geographic information system map, which can render the precise movement trajectory of the mobile equipment in real time in three-dimensional form, dynamically construct a three-dimensional model of the operation scene generated by the fused point cloud, and realize upper-level application functions such as automatic calculation of material pile volume and collision warning of mobile equipment on this basis.

[0021] In one specific embodiment, the BeiDou positioning module 10 is used to continuously acquire the three-dimensional position, three-dimensional velocity, attitude heading, and precise Coordinated-Universal-Time (UTC) timestamp of the mobile equipment in a preset global coordinate system (such as WGS-84 or Beijing 54 coordinate system).

[0022] The BeiDou positioning module 10 specifically includes a Global Navigation Satellite System (GNSS) receiver unit that supports real-time dynamic positioning (RTK) for multiple systems and multiple frequencies. The GNSS receiver unit includes one or more GNSS receiver terminals, a GNSS ground-based reference station (base station), and high-gain GNSS antennas connected to each receiver terminal. The GNSS receiver terminal is preferably an industrial-grade product capable of simultaneously receiving and processing signals from multiple constellations such as BeiDou Navigation Satellite System (BDS), Global Positioning System (GPS), GLONASS, and Galileo. The high-gain GNSS antenna, such as a choke coil antenna, has the ability to suppress multipath effects and is installed on the mobile equipment body in a position where the signal is not easily blocked by its own structure, such as the top center of large engineering machinery or the top of the operator's cab.

[0023] The working principle of the Beidou positioning module 10 is based on carrier phase differential technology. A GNSS ground-based reference station is set up at a known precise coordinate point, continuously receives satellite signals, and compares them with its own known coordinates to calculate in real time the comprehensive error caused by ionospheric / tropospheric delay, satellite orbit error, and satellite clock error. The ground-based reference station broadcasts the differential correction data generated by these error calculations to the GNSS receiver terminal installed on mobile equipment via a wireless data link (such as an UHF radio or a 4G / 5G mobile network). While receiving its own satellite signals, the receiver terminal receives and applies the differential correction data, and eliminates common-mode error by solving the integer ambiguity of the carrier phase, thereby obtaining centimeter-level real-time positioning accuracy. The use of multi-constellation fusion calculation increases the number of available satellites and optimizes the satellite geometric distribution, thereby enhancing the availability, continuity, and reliability of positioning calculation, especially in complex environments with partial obstruction, such as ports.

[0024] The BeiDou positioning module 10 further includes a positioning data preprocessing unit, which is implemented at the software or firmware level and is used to perform quality control and optimization on the data before it enters the data fusion processing module 30. The detailed processing steps of the positioning data preprocessing unit include: S101: Receive raw positioning data frames output in real time by the GNSS receiver terminal; each data frame contains a three-dimensional position coordinate vector. Three-dimensional velocity vector High-precision timestamp The solution quality is characterized by state parameters, including the type of positioning solution (e.g., fixed solution, floating-point solution, single-point solution) and the signal strength indices of each satellite involved in the solution, such as signal-to-noise ratio. ; S102: Preliminary screening is performed based on the positioning solution type; this positioning data preprocessing unit checks the solution type of each data frame and retains only the data frames with fixed solutions of the highest precision level; all positioning data with floating-point solutions, single-point solutions or other low-precision types are regarded as invalid observations and discarded directly to prevent low-quality data from polluting the subsequent fusion filtering process; S103: Secondary screening is performed based on signal quality indicators; for the fixed solution data screened through step S102, the positioning data preprocessing unit further evaluates its reliability; specifically, the average signal-to-noise ratio of all satellites participating in this solution is calculated; if the average signal-to-noise ratio is lower than a preset quality threshold (e.g., 35dB), it is considered that although the positioning result is a fixed solution, it was obtained in a weak signal environment, and its reliability is questionable, so it is also rejected; S104: Perform smoothing filtering on the valid location data sequences that have passed all filtering steps; this location data preprocessing unit uses a moving average filtering algorithm based on a sliding time window to suppress random high-frequency noise in the valid data; its specific implementation is as follows: in, This represents the current discrete time step. In time step The effective position coordinate vectors obtained through the above filtering steps; The value is the size of the sliding window, a positive integer, and its value can be configured according to the motion characteristics of the mobile equipment. In time step The output is a smoothed position coordinate vector.

[0025] The same filtering process is also applied to the velocity vector. After the above steps, the positioning data preprocessing unit outputs a cleaner, smoother, and more reliable positioning observation data stream for use by the data fusion processing module 30.

[0026] The laser scanning module 20 is used to acquire three-dimensional spatial point cloud data of the environment surrounding the mobile equipment in real time and to preprocess the raw point cloud data; the laser scanning module 20 provides high-quality, structured three-dimensional geometric information for subsequent data fusion and scene three-dimensional reconstruction.

[0027] In a preferred embodiment, the laser scanning module 20 includes at least two laser scanners and a dedicated point cloud data preprocessing industrial control computer; the laser scanners can be divided into two types according to their installation location and functional positioning on the mobile equipment.

[0028] The laser scanner has a 360-degree horizontal field of view and multi-echo recognition function. Some laser scanners are fixedly installed on the key moving parts of the mobile equipment, such as the two sides of the cantilever structure of the bucket wheel stacker-reclaimer, to collect fine local point clouds of the working area. Other laser scanners are installed on a high-precision two-dimensional servo gimbal and fixed at the top of the mobile equipment to receive and execute specific control commands from the data fusion processing module 30 to achieve targeted scanning of specific targets or areas.

[0029] The laser scanning module 20 further includes a point cloud data preprocessing industrial computer, which is a high-performance industrial computer running a point cloud processing software environment, such as software programs based on the Linux operating system and the Point-Cloud-Library (PCL). This industrial computer receives all the raw point cloud data output from the laser scanner and executes a series of preprocessing algorithms. The preprocessing process specifically includes the following steps: S201: Receive the output with precise timestamps from each laser scanner. Original 3D point cloud data frame .

[0030] S202: Perform Statistical Outlier Removal (SOR) filtering on the original 3D point cloud data frame. This step removes isolated noise points caused by airborne dust, water vapor, or the sensor itself. The principle is: for each point in the original 3D point cloud data frame... Calculate its distance to the neighborhood The average distance between the nearest neighbors Then, calculate the mean of the distribution formed by the average distances of all points. and standard deviation A point is considered an outlier if its average distance exceeds a threshold determined by global statistical characteristics. ;in: This is the point currently to be judged; For point To its The average distance between the nearest neighbors; This is the mean of the average distances between all points in the point cloud; is the standard deviation of the average distance of all points in the point cloud; The standard deviation multiple threshold is a configurable parameter.

[0031] All points that meet the above conditions are removed from the original 3D point cloud data frame, resulting in a cleaner point cloud. ; S203: Filtered point cloud Perform voxel downsampling; this step reduces the amount of point cloud data while maintaining the macroscopic geometry of the point cloud to meet the real-time requirements of subsequent algorithms. The principle is to divide the entire 3D space containing the point cloud into a mesh composed of numerous tiny cubes (i.e., voxels); for each non-empty voxel... All points within Calculate their geometric center (centroid). The centroid is used to represent all points within the voxel. The formula for calculating the centroid is: in: For the first One non-empty voxel; voxels The number of points contained within; voxels The first One point; The calculated center of mass of the voxel; The centroids of all voxels together constitute the final downsampled point cloud. After completing the above preprocessing steps, the point cloud data preprocessing industrial control computer will process the clean and structured point cloud data stream with original timestamps. Send to data fusion processing module 30.

[0032] like Figure 1 As shown, the data fusion processing module 30 can be physically deployed on one or more high-performance, high-reliability industrial-grade computers. To ensure uninterrupted operation 24 / 7 in harsh industrial environments, its hardware platform is preferably a server with redundant design, such as a dual- or multi-processor with fault tolerance, a large-capacity error-correction code (ECC) memory, and a redundant power supply. The data fusion processing module 30 receives data from the Beidou positioning module 10 and the laser scanning module 20, and executes a series of complex algorithms to output high-precision, high-reliability mobile equipment status and spatiotemporally registered global three-dimensional point cloud data.

[0033] In one specific embodiment, the data fusion processing module 30 is implemented collaboratively by multiple software units integrated within it; these software units include: an online calibration and monitoring unit, a spatiotemporal registration unit, an adaptive fusion algorithm unit, and a scanning strategy generation unit.

[0034] The online calibration and monitoring unit exists as a continuously running background process. Its function is to calibrate and monitor the relative pose relationships (extrinsic parameters) between the scanners in the laser scanning module 20 in real time and automatically, in order to compensate for installation errors caused by vibration of moving equipment, structural deformation, or temperature changes. The execution steps of the online calibration and monitoring unit include: S301: Based on the nominal mounting pose and field of view parameters of each scanner, calculate and identify in real time the overlapping area of ​​point cloud data collected by different scanners (such as long-range scanners and multi-layer scanners) in three-dimensional space.

[0035] S302: Within the identified overlapping region, stable geometric feature points are independently extracted from the two point cloud datasets and described using descriptors such as Fast-Point-Feature-Histograms (FPFH). Subsequently, a set of corresponding feature point pairs is established using robust matching algorithms such as random sampling consistency. .

[0036] S303: Calculate the optimal relative rotation matrix by solving a nonlinear least squares optimization problem. With translation vector The objective function is to minimize the reprojection error between all pairs of points with the same name. in: The optimal solution to this optimization problem, together constitute the solution from the scanner. Coordinate system to scanner Optimal rigid body transformation of the coordinate system. Among them, This is the optimal 3x3 rotation matrix. The optimal 3x1 translation vector; This represents an optimization operation aimed at finding parameters that minimize the objective function value. and This operation will solve for the rotation matrix. Constraints in a 3D special orthogonal group The inner set (i.e., the set of all 3x3 rotation matrices) and the translation vector to be solved. Constrained in three-dimensional real vector space Inside; This represents the total number of identical feature point pairs established in step S302. For from the scanner The Coordinates of one feature point; For it in the scanner The coordinates of the corresponding points with the same name in the table; This represents the square of the L2 norm (i.e., the Euclidean norm) of a vector; in this formula, it calculates the source point after rotation and translation transformations. Its corresponding target point The square of the Euclidean distance between them.

[0037] S304: The optimal extrinsic parameters obtained from the solution The data is updated in the device for use by the spatiotemporal registration unit. At the same time, the optimized root mean square error of the residual is calculated as a health indicator of the sensor's extrinsic parameters. If the indicator remains stable at a low level for a long period, it indicates that the sensor is in good installation condition. If the indicator shows a continuous increase or a sudden change, it is determined that the sensor's extrinsic parameters are out of sync. The data fusion processing module 30 will reduce the weight of its data in subsequent fusion and can issue a maintenance alarm to the data storage and visualization module 50.

[0038] The spatiotemporal registration unit is used to unify all heterogeneous and asynchronous sensor data under the same spatiotemporal reference. The execution steps of the spatiotemporal registration unit include: S311: Time synchronization, for any frame timestamp is... The point cloud data, through the timestamps of the two nearest BeiDou positioning modules 10, and Linear interpolation is performed between the points to obtain the pose of the moving equipment at the moment of point cloud acquisition, thereby achieving millisecond-level time alignment. S312: Spatial registration transforms each pre-processed point cloud frame from its respective sensor coordinate system to a unified global coordinate system. For any point cloud frame in the local coordinate system of the laser scanner... The point below In the global coordinate system coordinates below It is calculated using the following formula: in: For the coordinate system of the laser scanner relative to the mobile equipment body The external parameter, whose value is provided and updated in real time by the online calibration and monitoring unit; The pose of the mobile equipment body coordinate system in the global coordinate system is provided by the best state estimate output by the adaptive fusion algorithm unit at the previous moment.

[0039] The adaptive fusion algorithm unit employs the Extended-Kalman Filter (EKF) framework to optimally fuse multi-source data to estimate the complete state of the mobile equipment. First, the state vector of the device is defined. It includes at least the three-dimensional position of the moving equipment in the global coordinate system. 3D velocity and the unit quaternion representing attitude The filtering process consists of two stages: prediction and update.

[0040] During the prediction phase, based on the kinematic model of the mobile equipment, readings from the internal Inertial Measurement Unit (IMU) are used. This IMU measures the three-dimensional angular velocity and three-dimensional acceleration of the mobile equipment body, which is equipped with a three-dimensional measurement data fusion device, in real time at a high frequency (e.g., 100Hz or higher). The output data of the IMU is directly fed into the adaptive fusion algorithm unit as the basic input for performing the extended Kalman filter prediction step, providing crucial dynamic information for solving the kinematic model. Especially when the BeiDou positioning signal is briefly interrupted or its quality degrades, it can ensure the continuity and smoothness of the state estimation from the previous moment. State estimation Predict the current moment Prior state and its covariance in: For IMU at time The measured value; It is a nonlinear state transition function; for The Jacobian matrix for the state; Let be the process noise covariance matrix.

[0041] During the update phase, when new external observations (such as position observations from BeiDou positioning module 10) are available... Or pose observations obtained by matching laser point clouds with prior maps. Upon arrival, the prior estimate is corrected using the following formula to obtain the optimal posterior estimate. and its covariance : in: This represents the observation vector at the current moment; For the observation model function; for The Jacobian matrix for the state; Kalman gain; Represent an identity matrix; The key to this adaptive fusion algorithm unit lies in the observation noise covariance matrix. It is adaptively adjusted. Specifically, when the observation comes from Beidou positioning module 10, The element values ​​and the satellite signal-to-noise ratio calculated in this positioning solution Inversely proportional; when the observation comes from lidar, The element values ​​are inversely proportional to the goodness of fit of the point cloud registration and the sensor health index output by the online calibration and monitoring unit. In this way, the device automatically trusts high-quality observation data and suppresses low-quality observation data, achieving adaptive weighting in the fusion process.

[0042] The scanning strategy generation unit facilitates the realization of the active sensing capability of this invention. The execution steps of the scanning strategy generation unit include: S321: Real-time monitoring of the posterior state covariance matrix output by the adaptive fusion algorithm unit. The diagonal elements of this matrix The first one was directly quantified The variance of each state component (e.g., X-direction position, pitch angle), i.e. the degree of uncertainty.

[0043] S322: Combine the variance of each state component with a preset uncertainty threshold vector that is related to the task accuracy requirements. The comparison is performed; if the uncertainty of any state exceeds its threshold, i.e. If the positioning or orientation accuracy of the device in that degree of freedom is decreasing, active intervention is required.

[0044] S323: When active intervention is triggered, the scanning strategy generation unit will query the prior environment map (such as a GIS map or a historical 3D point cloud map) preloaded in the data storage and visualization module 50. Near the current estimated location of the mobile equipment, it will search for long-term stable environmental features that can most effectively constrain the current uncertainty of the largest dimension, such as the right-angled edge of a building, a fixed lighthouse, or a track line.

[0045] S324: Based on the global coordinates of the selected target features, the scanning strategy generation unit calculates the pitch and yaw angles that the long-range scanner in the laser scanning module 20 needs to point at, and generates corresponding control commands. These commands are then sent to the scanner via the data transmission module 40. The scanner executes the commands to perform a high-density, high-precision target-oriented scan. The obtained point cloud data, due to its definite source and clear target, is given extremely high weight (i.e., a very small observation noise is used during EKF updates). This allows for the rapid and significant reduction of state uncertainties, bringing the device back to a high-precision operating state.

[0046] See Figure 1The data transmission module 40 establishes a bidirectional data communication link between the various hardware modules inside the device (including the Beidou positioning module 10, the laser scanning module 20, the data fusion processing module 30, and the data storage and visualization module 50). The function of the data transmission module 40 is to ensure that the large-capacity three-dimensional point cloud data, high-frequency positioning data, and control commands and synchronization signals with extremely high real-time requirements generated by the device can be transmitted between the units with low latency and high reliability.

[0047] In a preferred embodiment, the data transmission module 40 employs a physically redundant and logically seamless communication architecture. At the physical level, this architecture deploys at least two independent communication links. The first link, serving as the primary link, utilizes high-bandwidth industrial-grade fiber optic communication. For scenarios requiring data exchange between rotating components of mobile equipment (e.g., the slewing mechanism of a stacker-reclaimer) and stationary components, this fiber optic link is connected via an industrial-grade fiber optic slip ring, ensuring stable transmission of fiber optic signals during continuous rotation of the mobile equipment. The second link, serving as a parallel backup link, employs industrial wireless communication technology, such as deploying enterprise-grade wireless access points (APs) and clients conforming to IEEE-802.11ac or higher standards, to provide high-bandwidth wireless network coverage.

[0048] At the logical level, to achieve highly reliable and zero-interruption switching between two physical links, this module employs a Parallel Redundancy Protocol (PRP), such as the protocol defined in the IEC 62439-3 standard. The implementation of this protocol relies on network adapters or switches that support it at both the data sending and receiving ends. Its specific working steps are as follows: S401: Any node that needs to send data (e.g., the data fusion processing module 30 mounted on a mobile device) copies each data frame it generates into two identical copies at the network layer; S402: The sending node sends out a copy of one of the data frames through the first physical link (i.e., the fiber optic link); S403: At the same time, the transmitting node concurrently transmits another identical copy of the data frame through the second physical link (i.e., the industrial wireless link); S404: At the receiving end (e.g., the data storage and visualization module 50 located in the central control room), its network interface simultaneously listens for data from two physical links. Upon receiving the first valid data frame from either link, the receiving node immediately forwards the data frame to the upper-layer application and records the unique identifier of the data frame; S405: When the receiving node subsequently receives a duplicate data frame with the same unique identifier from another link, it will recognize it as a redundant copy and discard it directly without passing it to the upper layer application.

[0049] With this parallel redundancy mechanism, when any link (such as an optical fiber link that is interrupted due to physical damage) fails, data transmission will not be interrupted because the receiving end can always receive data from another normal link (industrial wireless link). From the perspective of upper-layer applications, the data stream is completely continuous, thereby achieving fault recovery with zero switching time and ensuring extremely high availability of data transmission.

[0050] To further ensure the quality of service for different types of data transmission, the data transmission module 40 also implements a Quality of Service (QoS) policy. Specifically, by employing an industrial switch that supports Virtual Local Area Network (VLAN) technology, different priorities are assigned to different types of data streams. For example, for control commands generated by the scanning policy generation unit for controlling long-range scanners and synchronization signals for multi-sensor time synchronization, the device will mark their data frames with a higher priority code point (PCP). For 3D point cloud data streams with huge data volumes but not extremely sensitive to single-frame latency, a lower priority is assigned. When processing data forwarding, the switch will prioritize high-priority data frames to ensure that the transmission latency of critical control and synchronization information is minimized, even under extremely high network load.

[0051] like Figure 1 As shown, the data storage and visualization module 50 serves as the final output terminal and human-computer interaction interface of the device. It is used to permanently archive the results generated by the data fusion processing module 30 and to display and analyze them in real time in an intuitive three-dimensional form.

[0052] In a preferred embodiment, the data storage and visualization module 50 comprises one or more data storage servers and a 3D visualization platform; the hardware platform of the data storage server is preferably an enterprise-grade server configured with a large-capacity, high-read / write-speed disk array (e.g., RAID5 or RAID6) to ensure data redundancy backup and security; the server executes a clear data archiving strategy, the specific steps of which include: S501: The received data is classified and stored. The server creates different storage paths to store the unprocessed raw data collected by the Beidou positioning module 10 and the laser scanning module 20, as well as the final result data after fusion processing output by the data fusion processing module 30. This separate storage method facilitates later problem tracing, algorithm debugging and historical data playback. S502: Establish a structured data organization method. For raw data, organize folders according to the hierarchical structure of sensor type, mobile equipment number, and acquisition time. For result data, store the high-frequency output mobile equipment state vectors (including position, velocity, attitude, timestamp, etc.) in a time series database for quick retrieval and time series analysis. Store the massive global point cloud data in a file format with timestamps (e.g., PCD or LAS format). S503: Performs preset data lifecycle management. Based on storage capacity and business needs, it sets the data retention period. For example, all data for the most recent three months is retained on the online storage server for immediate access, while data older than three months is automatically transferred to lower-cost nearline or offline storage media for long-term archiving. The 3D visualization platform is a software application developed based on a real-time 3D graphics engine (such as Unity3D or Unreal Engine). As a client, the platform subscribes to the required data streams from a data storage server or directly from the data fusion processing module 30 via the data transmission module 40, and performs real-time rendering and functional implementation within a virtual 3D scene. The core functional implementation steps of this 3D visualization platform are as follows: S511: Load and display basic geographic information. When the 3D visualization platform starts, it first loads a 2D or 3D Geographic Information System (GIS) base map that is consistent with the global coordinate system used by this device. This base map can contain static environmental elements such as factory roads, building outlines, fixed and mobile equipment, providing geospatial context for subsequent dynamic data visualization. S512: Real-time rendering of the motion trajectory of moving equipment. The 3D visualization platform receives and parses the state vector of the moving equipment output by the adaptive fusion algorithm unit in real time, and then uses the position vector in each frame of the received state vector to... and attitude quaternions The 3D visualization platform updates the spatial pose of a preset 3D model of mobile equipment in a 3D scene in real time, thereby accurately and smoothly reproducing the actual motion state of the mobile equipment; at the same time, a trajectory line can be drawn behind the model to display its historical motion path. S513: Dynamically construct and update 3D scene models. The 3D visualization platform receives 3D point cloud data streams that have been unified into the global coordinate system, output by the spatiotemporal registration unit. The 3D visualization platform maintains a global 3D data structure, such as an octree or voxel mesh. When a new point cloud frame arrives, the 3D visualization platform inserts the points in it into this data structure to achieve incremental updates to the scene model. For areas that are repeatedly scanned, the new point cloud can be used to update or encrypt existing areas, thereby dynamically reflecting changes in the scene, such as the accumulation or retrieval process of a material pile. To obtain better visualization effects, the 3D visualization platform can use algorithms such as Marching-Cubes to generate triangular mesh models in real time based on this point cloud data structure, displaying the surface of irregular objects such as material piles in solid form. S514: Perform real-time collision warning. The 3D visualization platform defines virtual bounding boxes or more precise colliders for the 3D model of the mobile equipment and obstacles (other mobile equipment, buildings, etc.) identified from GIS maps or real-time point clouds in the virtual scene. The 3D visualization platform continuously detects at high frequency (e.g., 30 times per second) whether there is spatial interference or the distance is less than a preset safety threshold between the collider of the mobile equipment model and the collider of other objects in the scene. When a potential collision risk is detected, the 3D visualization platform immediately triggers an alarm, such as highlighting the collision risk area on the screen and issuing an audible alarm.

[0053] S515: Enables automatic calculation of the volume of the work object; the 3D visualization platform provides interactive tools that allow users to select the boundary range of a specific work object (such as a material pile) in a 2D map or 3D scene; after the user confirms the range, the 3D visualization platform performs the following calculations: First, it extracts all 3D point sets located within the user-defined boundary from the dynamically updated global 3D scene model; then, it defines a reference plane, which can be a pre-measured material yard ground elevation model or a user-specified horizontal plane; finally, it calculates the volume using a grid-based Digital Elevation Model (DEM) method, that is, projecting the selected point set onto a 2D horizontal grid, calculating the average height of the points in each grid cell, multiplying it by the area of ​​the grid cell, and then subtracting the height of the reference plane at that location to obtain the volume of each grid column. The total volume of the material pile is obtained by summing the volumes of all grid columns; the calculation results can be displayed in real time and updated periodically.

[0054] The implementation process of an adaptive collaborative three-dimensional measurement method provided by an embodiment of the present invention will be described in detail below with reference to the accompanying drawings. Figure 2 As shown, Figure 2This is a schematic diagram of a method flow according to an embodiment of the present invention, which mainly includes the following steps: Step S1: Device Initialization and Online External Parameter Calibration; In a specific embodiment, when the device starts up, the device initialization and online external parameter calibration steps are first executed. In this step, the data fusion processing module 30 loads the initial or previous calibration external parameters (including rotation and translation parameters) of each laser scanner from the local configuration file. At the same time, as an independent and continuously running thread in the background, the online calibration and monitoring unit is activated. The online calibration and monitoring unit concurrently performs a series of operations: acquiring data from the laser scanning module 20 in real time, automatically identifying overlapping areas in the fields of view of different scanners; extracting and matching robust geometric feature points in these overlapping areas; calculating the current optimal sensor external parameters based on the set of matched point pairs through optimization solution; finally, the online calibration and monitoring unit evaluates the health of the external parameters of each sensor based on the residual of the optimization results. This online calibration process is performed continuously throughout the entire operation of the device, ensuring the real-time accuracy of the external parameters.

[0055] Step S2: Concurrent Data Acquisition and Preprocessing. In this step, the BeiDou positioning module 10 and the laser scanning module 20 work independently and in parallel at high frequency. The BeiDou positioning module 10 acquires raw positioning data containing information such as position, velocity, and satellite signal quality through its GNSS receiver, and its internal positioning data preprocessing unit filters and smooths the data to output stable and reliable global positioning observations. At the same time, all laser scanners in the laser scanning module 20 acquire three-dimensional point cloud data within their respective fields of view and send the raw point cloud data to the point cloud data preprocessing industrial control computer in real time. The point cloud data preprocessing industrial control computer performs outlier removal and data downsampling operations to output a structured point cloud with lower noise and more concise data. Step S3: Perform real-time high-precision spatiotemporal registration; the spatiotemporal registration unit in the data fusion processing module 30 receives the preprocessed positioning data stream and point cloud data stream from the concurrent data acquisition and preprocessing step in the previous step S2; for each frame of point cloud data, the spatiotemporal registration unit calculates the mobile equipment pose accurate to the millisecond level at the moment the point cloud frame is acquired by interpolation algorithm based on its timestamp; subsequently, the spatiotemporal registration unit calls the latest external parameters updated in real time by the online calibration and monitoring unit in the device initialization and external parameter online calibration step in step S1, and combines it with the mobile equipment pose obtained by interpolation to accurately transform each point in the point cloud frame from its own local coordinate system to a unified global coordinate system, thus completing the spatiotemporal reference unification of all data; Step S4: Perform adaptive data fusion and state estimation; this is the core iterative loop of this method. Within a complete iteration cycle, the adaptive fusion algorithm unit first predicts the state (position, velocity, attitude, etc.) of the mobile equipment at the current moment based on the readings of the inertial measurement unit and the kinematic model of the mobile equipment, obtaining a prior state estimate and its uncertainty (covariance matrix). When a new spatiotemporally registered observation data (such as a GNSS positioning point or a pose obtained from lidar point cloud registration) arrives, the adaptive fusion algorithm unit does not update it immediately. Instead, it dynamically sets the noise covariance matrix of the observation based on the quality indicators of the observation data, such as the satellite signal-to-noise ratio of GNSS observations or the point cloud matching score and sensor health of lidar observations. A high-quality observation will be assigned a small noise value (high weight), and vice versa. After the noise setting is completed, the adaptive fusion algorithm unit performs the extended Kalman filter update step, combining the prior estimate and the weighted observation to calculate the optimal posterior state estimate at the current moment. and its updated covariance matrix ; Step S5: Uncertainty-based active scanning closed-loop control; This method executes this step in parallel with the above state estimation, triggering a condition; Under normal operating conditions, when the state covariance output by the adaptive fusion algorithm unit... Within the preset accuracy range, this active scanning closed-loop control step is in a silent monitoring state; the scanning strategy generation unit in the data fusion processing module 30 continuously checks the covariance matrix. The diagonal elements represent the variance of each state component. When the variance of any state component exceeds the preset task accuracy threshold, it indicates that the uncertainty of the device in that degree of freedom is growing uncontrollably, and the active scanning closed-loop control step is immediately activated. After activation, the scanning strategy generation unit queries prior map data to determine an environmental stability feature point that can most effectively constrain the uncontrollable state, and generates a target scanning command. This command is sent to the long-range scanner in the laser scanning module 20, driving it to accurately point to the target feature and perform a high-density scan. This actively acquired high-value observation data will be given extremely high weight and immediately sent back to the update stage of the adaptive data fusion and state estimation step S4, thus forming a complete closed loop of problem discovery, decision-making, active perception, and problem correction, quickly bringing the state uncertainty of the device back to a normal level. Step S6: Integration of data distribution, storage, and application steps; the final fusion result generated from the adaptive data fusion and state estimation steps in Step S4 is a high-frequency, high-precision mobile equipment state vector sequence. The global 3D point cloud, which has undergone precise spatiotemporal registration, is distributed to two destinations via the data transmission module 40. On the one hand, it is sent to the data storage server in the data storage and visualization module 50, where it is classified, archived, and stored long-term according to a predetermined strategy. On the other hand, it is pushed to the 3D visualization platform in real time. The 3D visualization platform uses this real-time data to render the precise trajectory of the mobile equipment in a 3D virtual environment, dynamically construct a 3D model of the work scene, and provide operators with advanced auxiliary functions such as real-time collision warning and automatic measurement of material pile volume.

[0056] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning, characterized in that, The three-dimensional measurement data fusion device includes: The BeiDou positioning module is used to obtain global positioning information of mobile equipment in the global coordinate system; An inertial measurement unit is used to acquire the three-dimensional angular velocity and three-dimensional acceleration information of the mobile equipment; A laser scanning module is used to acquire three-dimensional point cloud information of the environment surrounding the mobile equipment, and the laser scanning module includes multiple laser scanners, including long-range scanners and multi-layer scanners. The data fusion processing module is used to receive the global positioning information, three-dimensional angular velocity and three-dimensional acceleration information, and three-dimensional point cloud information. Based on the kinematic model, it estimates the state vector of the mobile equipment and the state covariance matrix representing the uncertainty of the state vector by fusing the global positioning information, the three-dimensional angular velocity and three-dimensional acceleration information, and the three-dimensional point cloud information. It monitors the state covariance matrix in real time. When the uncertainty of the state vector exceeds a preset uncertainty threshold, it generates control instructions for driving the long-range scanner to perform scanning of a specific area. The data transmission module is used to send the control command from the data fusion processing module to the laser scanning module to drive the long-range scanner to perform targeted scanning of stable environmental features, thereby acquiring observation data; The data storage and visualization module is used to store the state vector and update the state vector using the observation data. During the update process, an observation noise covariance matrix is ​​set for the observation data to accelerate the convergence of the uncertainty.

2. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The data fusion processing module includes a scanning strategy generation unit, which is used to generate control commands, specifically including: The diagonal elements of the state covariance matrix are monitored in real time to obtain the variance of each state component. When the variance of any state component exceeds the uncertainty threshold, a preset prior environment map is queried to determine the stable environment feature that can constrain the dimension with the largest current uncertainty. Based on the global coordinates of the stable environmental feature, a control command is generated to drive the long-range scanner to point at the stable environmental feature.

3. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The data fusion processing module further includes an adaptive fusion algorithm unit. The adaptive fusion algorithm unit adopts an extended Kalman filter framework and fuses the global positioning information, the three-dimensional angular velocity and three-dimensional acceleration information, and the three-dimensional point cloud information to estimate the state vector of the mobile equipment and the state covariance matrix representing the uncertainty of the state vector. The adaptive weighting of the fusion process is achieved by dynamically adjusting the observation noise covariance matrix to obtain the optimal state vector of the mobile equipment. When the observation data comes from the BeiDou positioning module, the element values ​​of the observation noise covariance matrix are inversely proportional to the satellite signal-to-noise ratio contained in the observation data. When the observation data comes from the laser scanning module, the element values ​​of the observation noise covariance matrix are inversely proportional to the goodness of fit of the 3D point cloud information registration.

4. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The data fusion processing module further includes: an online calibration and monitoring unit and a spatiotemporal registration unit; The online calibration and monitoring unit is used for: Real-time identification of overlapping areas in the 3D point cloud information collected by different laser scanners in the laser scanning module; Extract and match pairs of feature points with the same name within the overlapping region; By solving a nonlinear least squares optimization problem with the goal of minimizing reprojection error, and using the corresponding feature point pairs as input, the extrinsic parameters between each laser scanner are calculated in real time. The spatiotemporal registration unit is used to transform the three-dimensional point cloud information from their respective sensor coordinate systems to a unified global coordinate system using the extrinsic parameters.

5. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 4, characterized in that, The online calibration and monitoring unit is also used for: The root mean square error of the residual after solving the nonlinear least squares optimization problem is used as the health index of the external parameters of the laser scanner. In the adaptive fusion algorithm unit, when the observation data comes from the laser scanning module, the element values ​​of the observation noise covariance matrix are inversely proportional to the health index.

6. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The BeiDou positioning module includes a positioning data preprocessing unit, used to perform quality control before the data enters the data fusion processing module. The quality control includes: Based on the location solution type, only data frames with a fixed solution type are retained; A secondary screening is performed based on the average signal-to-noise ratio of each satellite involved in the calculation, and data frames with an average signal-to-noise ratio lower than a preset quality threshold are removed.

7. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The laser scanning module also includes a point cloud data preprocessing industrial control computer; The point cloud data preprocessing industrial control computer is used to perform statistical outlier removal filtering and voxel mesh downsampling to obtain structured 3D point cloud information.

8. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The long-range scanner is integrated into the two-dimensional pan-tilt unit; The multilayer scanner is fixedly installed on both sides of the cantilever structure of the mobile equipment of the three-dimensional measurement data fusion device.

9. The three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning according to claim 1, characterized in that, The data transmission module adopts a redundant communication architecture that combines optical fiber communication and industrial wireless communication. By implementing a parallel redundancy protocol, copies of data frames are transmitted concurrently through the primary link and the backup link, and the receiving end only processes the first data frame copy to arrive.

10. A method for fusing three-dimensional measurement data based on BeiDou positioning and laser scanning, applied to the three-dimensional measurement data fusion device based on BeiDou positioning and laser scanning as described in any one of claims 1-9, characterized in that, The method includes the following steps: S1. Collect global positioning information of the mobile equipment through the Beidou positioning module, and collect three-dimensional angular velocity and three-dimensional acceleration information of the mobile equipment through the inertial measurement unit; S2. Collect three-dimensional point cloud information of the environment surrounding the mobile equipment through the laser scanning modules of multiple laser scanners, including long-range scanners and multi-layer scanners. S3. Based on the kinematic model, and by integrating the global positioning information, the three-dimensional angular velocity and three-dimensional acceleration information, and the three-dimensional point cloud information, estimate the state vector of the mobile equipment and the state covariance matrix representing the uncertainty of the state vector; S4. Monitor the state covariance matrix in real time. When the uncertainty of the state vector is determined to exceed a preset uncertainty threshold based on the state covariance matrix, generate a control command to drive the long-range scanner to perform scanning of a specific area. S5. Send the control command to the laser scanning module to drive the long-range scanner to perform targeted scanning of stable environmental features, thereby acquiring observation data; S6. Update the state vector using the observation data, wherein, during the update process, an observation noise covariance matrix is ​​set for the observation data to accelerate the convergence of the uncertainty.

Images