Method and device for optimizing sensor parameters of heavy-load equipment, equipment and medium

Abstract

The invention provides an optimization method and device for sensor parameters of heavy-load equipment, equipment and a medium, and the optimization method comprises the steps: collecting the sensor data of the heavy-load equipment, and collecting the positioning information of the heavy-load equipment through a preset navigation system; calculating a camera projection error of the camera; calculating a point cloud matching error of the laser radar; calculating a motion data error of the inertial measurement equipment; and according to comparison results of the point cloud matching error and the corresponding threshold value, the camera projection error and the corresponding threshold value, and the motion data error and the corresponding threshold value, whether external parameters of the camera and external parameters of the laser radar need to be optimized is judged. According to the optimization method and device for the sensor parameters of the heavy-load equipment, the equipment and the medium, the technical problem that accumulated errors generated by the sensor cannot be eliminated can be solved.

Description

Technical Field

[0001] The present invention relates to the industrial field, and in particular to a method, device, equipment and medium for optimizing sensor parameters of heavy-load equipment. Background Art

[0002] With the continuous advancement of positioning technology, the demand for autonomous positioning and navigation of heavy-duty equipment in complex natural environments is increasing. Currently, existing sensors primarily rely on inertial navigation systems or global positioning systems to track the position of heavy-duty equipment. However, these systems can provide short-term positioning support during routine operations but cannot effectively adapt to the dynamic challenges of harsh working conditions.

[0003] When operating in harsh environments for extended periods, the accumulated errors generated by sensors in heavily loaded equipment cannot be eliminated, leading to a significant decrease in positioning accuracy. This not only affects the navigation reliability of the sensors but also limits their application scope and efficiency in heavy-load scenarios. Summary of the Invention

[0004] The present invention provides a method, device, equipment and medium for optimizing sensor parameters of heavy-duty equipment, so as to solve the technical problem that the accumulated error generated by the sensor cannot be eliminated.

[0005] The present invention provides a method for optimizing sensor parameters of heavy-load equipment, characterized by comprising:

[0006] Collect sensor data of the heavy-duty equipment through sensors, and collect positioning information of the heavy-duty equipment through a preset navigation system; the sensors include cameras, laser radars, and inertial measurement devices; the sensor data collected by the sensor parameters include target image data collected by each camera, target point cloud data collected by each laser radar, and motion data collected by the inertial measurement device;

[0007] Calculating a camera projection error of the camera based on target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a laser radar associated with the camera;

[0008] Calculating the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars;

[0009] Calculating a motion data error of the inertial measurement device based on the motion data and the positioning information;

[0010] According to the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold, it is determined whether the external parameters of the camera and the external parameters of the lidar need to be optimized.

[0011] In one embodiment of the present invention, the step of calculating the camera projection error of the camera based on the target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a laser radar associated with the camera includes:

[0012] Calculating projection image data based on the intrinsic and extrinsic parameters of the camera and target point cloud data of a laser radar associated with the camera;

[0013] A camera projection error of the camera is calculated based on the target image data and the projection image data of the camera.

[0014] In one embodiment of the present invention, the step of calculating the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars includes:

[0015] Calculate the product of the external parameter of the laser radar and the target point cloud data of other laser radars;

[0016] The point cloud matching error of the laser radar is calculated based on the target point cloud data of the laser radar and the product.

[0017] In one embodiment of the present invention, the step of calculating the motion data error of the inertial measurement device based on the motion data and the positioning information includes:

[0018] The motion data error of the inertial measurement device is calculated according to the positioning information and the position information in the motion data.

[0019] In one embodiment of the present invention, the step of determining whether it is necessary to optimize the camera extrinsic parameters and the lidar extrinsic parameters based on the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold includes:

[0020] Determine the matching error of each point cloud and the corresponding threshold, the projection error of each camera and the corresponding threshold, and the motion data error and the corresponding threshold:

[0021] When all point cloud matching errors are less than the corresponding threshold, all camera projection errors are less than the corresponding threshold, and the motion data error is less than the corresponding threshold, there is no need to optimize the extrinsic parameters of the camera and the extrinsic parameters of the lidar;

[0022] Otherwise, the external parameters of the camera and the external parameters of the lidar are optimized.

[0023] In one embodiment of the present invention, the step of optimizing the extrinsic parameters of the camera and the extrinsic parameters of the lidar includes:

[0024] Based on a preset objective function, a total residual is calculated according to the point cloud data error, the camera projection error, and the motion data error; the objective function represents a functional relationship between the point cloud data error, the camera projection error, the motion data error, and the total residual;

[0025] Solving the objective function, and when the objective function reaches a minimum value, obtaining updated data of the corresponding camera's extrinsic parameters and updated data of the lidar's extrinsic parameters;

[0026] The external parameters of the camera are optimized according to the updated data of the external parameters of the camera, and the external parameters of the laser radar are optimized according to the updated data of the external parameters of the laser radar.

[0027] In one embodiment of the present invention, before the step of collecting sensor data of the heavy-load equipment through the sensor and collecting positioning information of the heavy-load equipment through the preset navigation system, the sensor is further optimized. The preliminary optimization step includes:

[0028] The corresponding initial image data collected by each camera, the corresponding initial point cloud data collected by each lidar, and the initial motion data collected by the inertial measurement device;

[0029] Processing the initial image data and the initial point cloud data respectively according to the initial motion data, and calculating the corresponding image delay duration and point cloud delay duration;

[0030] Perform preliminary optimization processing on the corresponding camera according to the delay time of each image to obtain a preliminary optimized camera;

[0031] The corresponding lidar is preliminarily optimized according to the delay time of each point cloud to obtain a preliminarily optimized lidar.

[0032] The present invention also provides a device for optimizing sensor parameters of heavy-load equipment, comprising:

[0033] A data acquisition module is used to collect sensor data of the heavy-duty equipment and the positioning information of the heavy-duty equipment; the sensor data includes target image data collected by each camera, target point cloud data collected by each lidar, and motion data collected by the inertial measurement device;

[0034] a first calculation module, configured to calculate a camera projection error of the camera based on target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a laser radar associated with the camera;

[0035] A second calculation module is used to calculate the point cloud matching error of the laser radar based on the target point cloud data and external parameters of the laser radar and the target point cloud data of other laser radars;

[0036] a third calculation module, configured to calculate a motion data error of the inertial measurement device based on the motion data and the positioning information;

[0037] The external parameter optimization module is used to determine whether it is necessary to optimize the external parameters of the camera and the external parameters of the lidar based on the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold.

[0038] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the method for optimizing sensor parameters of the heavy-load equipment are implemented.

[0039] The present invention also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps of the method for optimizing sensor parameters of heavy-load equipment are implemented.

[0040] The beneficial effects of the present invention are as follows: based on the vision-lidar-IMU joint optimization model, the external parameters of the camera and the lidar are simultaneously optimized to effectively suppress the accumulation of multi-sensor errors; combined with the real-time online calibration mechanism, the external parameters of the camera and the lidar are iteratively optimized to continuously reduce the point cloud matching error, camera projection error and motion data error to below the preset threshold, ensuring that spatial consistency is maintained in harsh environments such as vibration and wind and sand. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The accompanying drawings are incorporated into and constitute a part of the specification, illustrate embodiments consistent with the present application, and together with the specification, are used to explain the principles of the present application. Obviously, the drawings described below are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be derived from these drawings without inventive effort.

[0042] In the attached figure:

[0043] Figure 1 A flowchart of a method for optimizing sensor parameters of heavy-load equipment provided by one embodiment of the present invention;

[0044] Figure 2 A schematic diagram of a sensor parameter optimization device for heavy-load equipment provided in one embodiment of the present invention;

[0045] Figure 3 A schematic diagram of an electronic device provided in one embodiment of the present invention.

[0046] The accompanying drawings are marked as follows: 100, data acquisition module; 200, first calculation module; 300, second calculation module; 400, third calculation module; 500, external parameter optimization module; 10, electronic device; 11, memory; 12, processor. DETAILED DESCRIPTION

[0047] The following describes the embodiments of the present invention through specific examples. Those skilled in the art will readily understand the other advantages and benefits of the present invention from the disclosure herein. The present invention may also be implemented or applied through various other specific embodiments. The details in this specification may be modified or altered based on different viewpoints and applications without departing from the spirit of the present invention. The following embodiments and features therein may be combined with one another without conflict.

[0048] It should be noted that the illustrations provided in the following embodiments are merely schematic illustrations of the basic concept of the present invention. The drawings only show components related to the present invention and are not drawn according to the number, shape, and size of components in actual implementation. In actual implementation, the type, quantity, and proportion of each component may be changed arbitrarily, and the component layout may also be more complex.

[0049] In the following description, numerous details are discussed to provide a more thorough explanation of the embodiments of the present invention. However, it will be apparent to those skilled in the art that the embodiments of the present invention may be practiced without these specific details. In other embodiments, well-known structures and devices are shown in block diagram form rather than in detail to avoid obscuring the embodiments of the present invention.

[0050] See also Figure 1 The present invention discloses a method for optimizing sensor parameters for heavy-duty equipment, which can optimize the parameters of sensors installed on the heavy-duty equipment, wherein the parameters can include external parameters and internal parameters. The optimization method can include the following steps: Step S10, collecting sensor data of the heavy-duty equipment via sensors, and collecting positioning information of the heavy-duty equipment via a preset navigation system; the sensor data collected by the sensors includes target image data collected by each camera, target point cloud data collected by each lidar, and motion data collected by an inertial measurement device.

[0051] In some embodiments, sensors and a pre-set navigation system are two independent data sources that operate simultaneously to collect data related to the positioning of heavy-duty equipment in real time. Sensor data can be raw data, such as target point cloud data from a lidar, target image data from a camera, or motion data from an inertial measurement unit (IMU). Positioning information refers to results or signals that can be directly used for positioning, such as latitude and longitude coordinates provided by a GPS or raw satellite signals.

[0052] In some embodiments, the number and type of sensors can be multiple, including multiple lidars, multiple cameras, inertial measurement devices, etc. The lidar can collect target point cloud data, that is, a three-dimensional point set of the environment surrounding the heavy-duty equipment, for sensing the structure of the environment. The camera can collect target image data, that is, visual information of the environment surrounding the heavy-duty equipment, for identifying features or scenes. The inertial measurement device can collect motion data, including the speed, acceleration, angular velocity, etc. of the heavy-duty equipment, for estimating the motion state of the heavy-duty equipment. The above data are all raw data and need to be processed by algorithms (such as SLAM, filtering, etc.) to obtain the sensor data of the heavy-duty equipment.

[0053] In some embodiments, when collecting target point cloud data, target image data, and motion data, a precise timestamp can be embedded in the above data to mark each frame of data with a high-precision timestamp, ensuring that each frame of data has a unique time identifier.

[0054] In some embodiments, before executing step S10, the sensors need to be initialized. That is, the lidar, camera, and inertial measurement device need to be initially optimized to ensure coordinate system alignment and data consistency between the sensors. Initialization includes initialization of individual sensors (e.g., each camera, each lidar) and initialization between sensors (e.g., camera and lidar, lidar and inertial measurement device).

[0055] In some embodiments, the step of performing preliminary sensor optimization may include performing preliminary optimization on each camera. Specifically, during the preliminary optimization of each camera, a planar checkerboard calibration plate may be used to analyze image data collected by the camera using the calibration plate to calculate the camera's intrinsic parameters. These parameters may include focal length, principal point coordinates, distortion coefficients, and the like. By performing preliminary optimization on each camera, it is ensured that the camera can accurately convert pixel positions in the image to positions in physical space.

[0056] In some embodiments, the step of performing preliminary optimization on the sensor may further include: performing preliminary optimization on each laser radar. Specifically, when performing preliminary optimization on each laser radar, multiple laser radars (such as A and B) can be allowed to scan the static scene at the same time to obtain their respective point cloud data. Subsequently, the coplanar planes or significant geometric structures in the point cloud are extracted, and the corresponding relationship is found through feature matching. Use the ICP algorithm (iterative closest point algorithm) to calculate the optimal rigid body transformation to transform the point cloud of laser radar B to the coordinate system of laser radar A. If there are multiple laser radars, select one of them (such as A) as the benchmark, or use the preset reference point cloud data as the benchmark, and calculate the external parameters of other radars relative to the benchmark radar / reference point cloud data in turn. By performing preliminary optimization on each laser radar, it can be ensured that the data of multiple laser radars are aligned in the same coordinate system.

[0057] In some embodiments, the step of performing preliminary optimization of the sensor may further include: performing preliminary optimization of the camera and the lidar. Specifically, when performing preliminary optimization of the camera and the lidar, data of the camera and the lidar on the planar checkerboard calibration plate may be collected, and the data of the camera and the lidar may be time-synchronized. The pixel positions of the feature points of the calibration plate in the image and the point cloud positions of the calibration plate in the lidar are extracted. The external parameters from the camera to the lidar are calculated using the PnP algorithm (Perspective-n-Point algorithm). By performing preliminary optimization of the camera and the lidar, the image data of the camera can be aligned with the point cloud data of the lidar to achieve the fusion of vision and lidar.

[0058] In some embodiments, the step of preliminary optimization of the sensor may further include: preliminary optimization of the inertial measurement device and the laser radar. Specifically, when preliminary optimizing the inertial measurement device and the laser radar, the heavy-duty equipment can be rotated in place on a flat ground, and the motion data measured by the inertial measurement device and the motion data calculated by the laser radar through point cloud matching are recorded. Based on the hand-eye calibration model AX=XB, let the transformation of the inertial measurement device between the two movements be A, and the corresponding transformation calculated by the laser radar be B, and solve the static transformation X from the inertial measurement device to the laser radar. By preliminary optimizing the inertial measurement device and the laser radar, the motion data of the inertial measurement device can be aligned with the coordinate system of the laser radar, thereby realizing the fusion of inertial navigation and the laser radar.

[0059] In some embodiments, the step of performing preliminary sensor optimization may further include performing preliminary optimization of the camera and inertial measurement device. Specifically, during the preliminary optimization of the camera and inertial measurement device, the camera-to-inertial measurement device extrinsic parameters can be obtained by multiplying the camera-to-lidar extrinsic parameters obtained above with the inertial measurement device-to-lidar extrinsic parameters. By performing preliminary optimization of the camera and inertial measurement device, the camera data can be aligned with the coordinate system of the inertial measurement device, achieving the fusion of visual and inertial navigation.

[0060] In some embodiments, the step of performing preliminary optimization on the sensor may further include: collecting corresponding initial image data through each camera, collecting corresponding initial point cloud data through each lidar, and collecting initial motion data through the inertial measurement device.

[0061] In some embodiments, after obtaining the initial image data, initial point cloud data, and initial motion data, all initial point cloud data and all initial image data can be compensated according to the initial motion data to obtain compensated initial point cloud data and initial image data.

[0062] In some embodiments, the initial motion data is used to correct and compensate all initial point cloud data and all initial image data, which can eliminate the impact of the movement of the heavy-loaded equipment on data acquisition. Specifically, the initial motion data can be used to calculate the rotation and translation of the heavy-loaded equipment during the lidar scanning and camera exposure through numerical integration. Numerical integration refers to integrating the acceleration and angular velocity data in the initial motion data collected by the inertial measurement device to calculate the rotation and translation of the heavy-loaded equipment during the lidar scanning and camera exposure. The rotation amount refers to the posture change of the heavy-loaded equipment obtained by integrating the angular velocity data. The translation amount refers to the position change of the heavy-loaded equipment obtained by integrating the acceleration data.

[0063] In some embodiments, assuming that the heavy-duty equipment is a rigid body during motion (i.e., the relative positions of the components inside the heavy-duty equipment remain unchanged), all initial point cloud data and all initial image data are uniformly converted to a coordinate system at a fixed time. A fixed time refers to the selection of a reference time (such as the start time of a lidar scan or the start time of a camera exposure). The conversion method can be expressed as a geometric transformation of all initial point cloud data and all initial image data based on the rotation and translation calculated by the inertial measurement device, so that they reflect the state of the heavy-duty equipment at the fixed time.

[0064] In some embodiments, the initial point cloud data and initial image data collected by the heavy-duty equipment during motion may be distorted due to the heavy-duty equipment's own motion. For example, the movement of the heavy-duty equipment during a LiDAR scan can cause the initial point cloud data to stretch or compress. Alternatively, the movement of the heavy-duty equipment during a camera exposure can cause the initial image data to appear blurry or smear. Motion compensation can be used to unify the initial point cloud data and initial image data into the same coordinate system at the same time, eliminating data distortion caused by the heavy-duty equipment's motion.

[0065] In some embodiments, the step of preliminarily optimizing the sensor may further include: processing the initial image data and the initial point cloud data respectively according to the initial motion data, and calculating the corresponding image delay duration and point cloud delay duration.

[0066] In some embodiments, the visual odometry can extract feature points (such as SIFT, ORB) from the initial image data and match the feature points in the initial image data of adjacent frames. Through the correspondence between the feature points, the relative motion data of the heavy-loaded equipment between the two adjacent frames of initial image data is calculated.

[0067] In some embodiments, for each laser radar, the corresponding relative motion data can be obtained according to the point cloud matching algorithm. Specifically, the point cloud matching algorithm may include an ICP algorithm and an NDT algorithm. ICP (Iterative Closest Point Algorithm) refers to matching the corresponding points in two adjacent initial point cloud data, and then iteratively calculating and applying rotation and translation to minimize the square error of the distance between the corresponding points, thereby calculating the relative motion data of the heavy-loaded equipment between the two frames. NDT (Normal Distribution Transformation) refers to dividing the target point cloud into multiple voxels, fitting a Gaussian distribution (mean + covariance) to each voxel, and then iteratively optimizing the probability likelihood of the source point cloud on these voxel Gaussian distributions to maximize the likelihood function and calculate the relative motion data of the heavy-loaded equipment.

[0068] In some embodiments, based on a cross-correlation model, the image delay duration corresponding to the maximum value of the correlation coefficient between the relative motion data and the corresponding initial motion data for each camera between two adjacent frames of initial image data can be calculated, and the point cloud delay duration corresponding to the maximum value of the correlation coefficient between the initial point cloud data and the corresponding initial motion data for each lidar can be calculated. The cross-correlation model represents the functional relationship between the relative motion data, the initial motion data, and the correlation coefficient.

[0069] In some embodiments, a cross-correlation model can be used to analyze the similarity between two signals (such as relative motion data and initial motion data). The correlation coefficient can be used to measure the similarity between two signals, and the value range is [-1, 1], where 1 indicates a perfect positive correlation, -1 indicates a perfect negative correlation, and 0 indicates no correlation. The image delay duration can be expressed as the time delay between the data collected by the camera and the data collected by the IMU. The point cloud delay duration can be expressed as the time delay between the data collected by the lidar and the data collected by the IMU.

[0070] In some embodiments, the cross-correlation model may be expressed as: Among them, τ * It represents the point cloud delay time or image delay time corresponding to the maximum value of the correlation coefficient; τ represents the different delay time settings; t represents time; Represented as initial motion data; Represented as relative motion data.

[0071] In some embodiments, a cross-correlation model can be used to calculate the correlation coefficient of two signals at different time delays. By adjusting the delay duration, the correlation coefficient can be calculated at different delay durations. The delay duration that maximizes the correlation coefficient can then be found, i.e., the point cloud delay duration or the image delay duration.

[0072] In some embodiments, the step of performing preliminary optimization on the sensor may also include: performing preliminary optimization processing on the corresponding camera according to the delay time of each image to obtain a preliminary optimized camera; performing preliminary optimization processing on the corresponding lidar according to the delay time of each point cloud to obtain a preliminary optimized lidar.

[0073] In some embodiments, the camera's timestamp is added or subtracted with the image delay time to align it with the motion data in time. The camera after preliminary optimization eliminates the time deviation and is closer to the real scene. The lidar's timestamp is added or subtracted with the point cloud delay time to align it with the initial motion data in time. The lidar after preliminary optimization eliminates the time deviation and is closer to the real scene. By comparing the data of each sensor (camera and lidar), it is verified that after the timestamp is initially optimized, the sensor data has achieved millisecond-level synchronization in time. On the basis of time synchronization, multi-sensor data is fused and optimized to improve the accuracy of positioning and navigation. After the timestamp optimization, the data of each sensor is accurately aligned in time, providing a reliable time benchmark for joint optimization.

[0074] In some embodiments, the optimization method may further include the following steps: Step S20, calculating the camera projection error of the camera based on the target image data, internal parameters, external parameters of the camera, and the target point cloud data of the laser radar associated with the camera.

[0075] In some embodiments, step S20 may include the following steps: step S21, calculating projection image data based on the intrinsic parameters and extrinsic parameters of the camera, and the target point cloud data of the laser radar associated with the camera.

[0076] In some embodiments, since there are multiple cameras and lidars, the cameras and lidars can be associated in advance. For example, the observation range of each camera can be overlapped and matched with the observation ranges of all lidars, and the lidar with the highest degree of overlap with the observation range of each camera can be obtained, and then matched with the corresponding camera.

[0077] In some embodiments, a camera's observation range refers to the camera's field of view in three-dimensional space, typically determined by the field of view (FOV) and detection range. A lidar's observation range refers to the lidar's scanning range in three-dimensional space, typically determined by the scanning angle and detection range. Overlap matching involves calculating the overlapping area between the camera's observation range and each lidar's observation range in three-dimensional space and evaluating the degree of matching between the two.

[0078] In some embodiments, the degree of overlap refers to the size or ratio of the overlapping area between the camera's observation range and the lidar's observation range in three-dimensional space. The lidar with the highest degree of overlap with the camera's observation range is selected as the sensor that matches the camera. Pairing the selected lidar with the camera to form a camera-lidar combination provides a foundation for subsequent multi-sensor data processing.

[0079] In some embodiments, camera intrinsic parameters may include focal length, principal point, and distortion coefficients. Focal length refers to the focal length of the camera in the x-axis and y-axis directions, principal point refers to the coordinates of the center point of the camera image plane, and distortion coefficients refer to the distortion characteristics of the camera lens. Camera extrinsic parameters may include rotation matrices and translation vectors. Rotation matrices describe the rotation of the camera relative to the world coordinate system, and translation vectors describe the translation of the camera relative to the world coordinate system.

[0080] In some embodiments, the projected image data refers to two-dimensional image data generated by projecting three-dimensional point cloud data onto a two-dimensional image plane of a camera.

[0081] In some embodiments, step S20 may further include the following steps: Step S22 , calculating a camera projection error of the camera according to the target image data and the projection image data of the camera.

[0082] In some embodiments, by finding the same feature points on the target image data and the projected image data, the camera projection error of each camera can be calculated based on the deviation of the feature points on the target image data and the projected image data. The calculation formula of the camera projection error is: ε cam (t) = z cam (t)-K×(T cam (t)×P cam ), where ε cam (t) represents the camera projection error at time t, z cam (t) represents the pixel coordinates of the feature point in the target image data at time t; K represents the camera intrinsic parameter; T cam (t) represents the three-dimensional coordinates of the feature point in the target point cloud data at time t; P cam Represented as camera extrinsics.

[0083] In some embodiments, the optimization method may further include the following steps: Step S30, calculating the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars.

[0084] In some embodiments, step S30 may include the following steps: calculating the product of the external parameter of the laser radar and the target point cloud data of other laser radars; and calculating the point cloud matching error of the laser radar based on the target point cloud data and the product of the laser radar.

[0085] In some embodiments, a point cloud matching model can be used to describe the relationship between the extrinsic parameters of a lidar, target point cloud data, and point cloud matching error. The extrinsic parameters of a lidar refer to the parameters that describe the position and posture of the lidar in three-dimensional space, including rotation matrices and translation vectors. The rotation matrix describes the rotation of the lidar relative to the world coordinate system. The translation vector describes the translation of the lidar relative to the world coordinate system. The point cloud matching error refers to the error in the alignment of target point cloud data from different lidars, which can be quantified by the distance between point clouds or the size of the overlapping area.

[0086] In some embodiments, the point cloud matching error can be expressed as: lidar (t) = f ICP (z lidar (t),T lidar (t)×p lidar ). Among them, ε lidar (t) represents the point cloud matching error at time t; Represents the point cloud matching model; z lidar (t) represents the three-dimensional coordinates of the feature point in the target point cloud data of a certain laser radar at time t; T lidar(t) represents the three-dimensional coordinates of the feature points in the target point cloud data of other laser radars at time t; p lidar Represents the extrinsic parameters of a certain lidar.

[0087] In some embodiments, the optimization method may further include the following steps: Step S40: calculating the motion data error of the inertial measurement device based on the motion data and the positioning information. Specifically, the motion data error of the inertial measurement device may be calculated based on the positioning information and the position information in the motion data.

[0088] In some embodiments, the motion data collected by the inertial measurement device can be expressed as:

[0089] v k+1 =v k +(R k a k +g)×Δt, Among them, R represents the rotation matrix; v represents the velocity data; P represents the position information; k represents the time; ω k represents the angular velocity in the motion data at time k; a k represents the acceleration in the motion data at time k; [ω k ] x Represents ω k The antisymmetric matrix of ; g represents the acceleration of gravity; Δt represents the time difference between time k and time k+1.

[0090] In some embodiments, the motion data error refers to the difference between the position information and the positioning information in the motion data collected by the IMU, which is used to evaluate the measurement accuracy of the IMU. The motion data error is expressed as: IMU (t) = z IMU (t)-f IMU (Δt). Where, ε IMU (t) represents the motion data error at time t; z IMU (t) represents the positioning information at time t; f IMU (Δt) represents the position information in the motion data at time t.

[0091] In some embodiments, the optimization method may further include the following steps: Step S50, based on the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold, determine whether it is necessary to optimize the external parameters of the camera and the external parameters of the lidar.

[0092] In some embodiments, step S50 may include the following steps:

[0093] Determine the matching error of each point cloud and the corresponding threshold, the projection error of each camera and the corresponding threshold, and the motion data error and the corresponding threshold:

[0094] When all point cloud matching errors are less than the corresponding threshold, all camera projection errors are less than the corresponding threshold, and motion data errors are less than the corresponding threshold, there is no need to optimize the camera's extrinsic parameters and the lidar's extrinsic parameters.

[0095] Otherwise, optimize the camera's external parameters and the lidar's external parameters.

[0096] In some embodiments, the point cloud matching error refers to the error in the alignment of point cloud data collected by different lidars, reflecting the accuracy of the external parameters (rotation matrix and translation vector). The camera projection error refers to the projection error between the image taken by the camera and the three-dimensional model (such as a point cloud or map), reflecting the accuracy of the camera external parameters. The motion data error refers to the error between the motion data of the inertial measurement device and the predicted position change information, reflecting the accuracy of the IMU external parameters. After the initial optimization of the sensor, the external parameters should remain accurate and the error should be maintained at a low level. By real-time monitoring of the changing trends of the point cloud matching error, camera projection error and motion data error, an error threshold is set according to the application scenario and equipment accuracy requirements. If the threshold is exceeded, the external parameters are considered inaccurate. When the error exceeds the preset threshold, the recalibration process is triggered to optimize the external parameters of the camera and the external parameters of the lidar.

[0097] In some embodiments, the step of optimizing the external parameters of the camera and the external parameters of the lidar may include: calculating the total residual based on the point cloud data error, camera projection error, and motion data error based on a preset objective function; the objective function characterizes the functional relationship between the point cloud data error, camera projection error, motion data error, and the total residual.

[0098] In some embodiments, the objective function can be expressed as: J(x)=∑ω cam ||ε cam || 2 +∑ω ICP ||ε ICP || 2 +∑ω IMU ||ε IMU || 2 Among them, ω cam 、ω ICP 、ω IMU They represent the weights of camera projection error, point cloud data error, and motion data error respectively; x represents the state variable, including the external parameters of the camera and the external parameters of the lidar; J(x) can be expressed as the total residual.

[0099] In some embodiments, the step of optimizing the external parameters of the camera and the external parameters of the lidar may also include: solving the objective function, and when the objective function reaches a minimum value, obtaining the corresponding updated data of the external parameters of the camera and the updated data of the external parameters of the lidar.

[0100] In some embodiments, the normal equation model can be used to solve the mathematical model of the least squares problem, which is expressed as: (J T J+λI)Δx=-J T ε. J represents the first-order partial derivative matrix of the total residual; T represents the matrix transpose; λ represents the adjustment parameter of the Levenberg-Marquardt (LM) algorithm; I represents the identity matrix; ε represents the residual vector consisting of all point cloud matching errors, all camera projection errors, and all motion data errors; and Δx represents the update data. The Levenberg-Marquardt (LM) algorithm can be used to optimize the objective function. State variables (such as extrinsic parameters) can be initially optimized to historical error data.

[0101] In some embodiments, the step of optimizing the external parameters of the camera and the external parameters of the lidar may further include: optimizing the external parameters of the camera according to the updated data of the external parameters of the camera, and optimizing the external parameters of the lidar according to the updated data of the external parameters of the lidar.

[0102] In some embodiments, the update data refers to extrinsic parameter adjustment values ​​calculated using a normal equation model or an optimization algorithm (such as the LM algorithm). The update data is used to adjust the extrinsic parameters of the lidar and camera (such as the rotation matrix and translation vector) to reduce the total residual. Through multiple iterations, the extrinsic parameters are gradually adjusted until the total residual converges to a global minimum.

[0103] In some embodiments, the external parameters of the lidar and the camera are optimized according to the updated data, and the point cloud matching error of each lidar after optimization and the corresponding threshold, the camera projection error of each camera and the corresponding threshold, and the motion data error and the corresponding threshold are repeatedly judged until the point cloud matching error of each lidar after optimization is less than the corresponding threshold, the camera projection error of each camera is less than the corresponding threshold, and the motion data error is less than the corresponding threshold.

[0104] It can be seen that in the above scheme, based on the vision-lidar-IMU joint optimization model, the external parameters of the camera and lidar are optimized synchronously to effectively suppress the accumulation of multi-sensor errors; combined with the real-time online calibration mechanism, the external parameters of the camera and lidar are iteratively optimized to continuously reduce the point cloud matching error, camera projection error and motion data error to below the preset threshold, ensuring spatial consistency in harsh environments such as vibration and wind and sand.

[0105] See also Figure 2 The present invention also discloses a device for optimizing sensor parameters of heavy-duty equipment. The above-mentioned optimization method can be applied to the device. The device can include: a data acquisition module 100, a first calculation module 200, a second calculation module 300, a third calculation module 400, and an external parameter optimization module 500.

[0106] In some embodiments, the data acquisition module 100 can be used to collect sensor data of heavy-loaded equipment and positioning information of heavy-loaded equipment; the sensor data includes target image data collected by each camera, target point cloud data collected by each lidar, and motion data collected by inertial measurement equipment.

[0107] In some embodiments, the first calculation module 200 may be used to calculate the camera projection error of the camera based on the target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a lidar associated with the camera.

[0108] In some embodiments, the second calculation module 300 can be used to calculate the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars.

[0109] In some embodiments, the third calculation module 400 may be configured to calculate a motion data error of the inertial measurement device based on the motion data and the positioning information.

[0110] In some embodiments, the extrinsic parameter optimization module 500 can be used to determine whether it is necessary to optimize the extrinsic parameters of the camera and the extrinsic parameters of the lidar based on the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold.

[0111] The specific definition of the optimization device can be found in the definition of the optimization method above and will not be repeated here. Each module in the above-mentioned optimization device can be implemented in whole or in part by software, hardware, or a combination thereof. Each of the above-mentioned modules can be embedded in or independent of the memory of the electronic device in the form of hardware, or can be stored in the memory of the electronic device in the form of software, so that the memory can call and execute the operations corresponding to each of the above modules.

[0112] See also Figure 3 In one embodiment, the electronic device 10 may include a memory 11, a processor 12, and a bus, and may also include a computer program stored in the memory 11 and executable on the processor 12, such as a program for the optimization method.

[0113] In one embodiment, the memory 11 includes at least one type of readable storage medium, and the readable storage medium includes a flash memory, a mobile hard disk, a multimedia card, a card-type memory (for example, an SD or DX memory, etc.), a magnetic memory, a magnetic disk, an optical disk, etc. In some embodiments, the memory 11 can be an internal storage unit of the electronic device 10, such as a mobile hard disk of the electronic device 10. In other embodiments, the memory 11 can also be an external storage device of the electronic device 10, such as a plug-in mobile hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card, etc. equipped on the electronic device 10. Furthermore, the memory 11 can also include both an internal storage unit of the electronic device 10 and an external storage device. The memory 11 can be used not only to store application software and various types of data installed in the electronic device 10, such as the code of the optimization method, but also to temporarily store data that has been output or is to be output.

[0114] In one embodiment, the processor 12 may be comprised of an integrated circuit, such as a single packaged integrated circuit or a plurality of packaged integrated circuits with the same or different functions, including a combination of one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and various control chips. The processor 12 is the control core (Control Unit) of the electronic device 10, connecting the various components of the entire electronic device 10 using various interfaces and circuits. It executes or runs programs or modules (such as optimized programs) stored in the memory 11 and calls data stored in the memory 11 to perform various functions of the electronic device 10 and process data.

[0115] In one embodiment, the processor 12 executes the operating system and various installed applications of the electronic device 10. The processor 12 executes the applications to implement the steps in the above-mentioned optimization method.

[0116] In one embodiment, the computer program can be divided into one or more modules, one or more of which are stored in the memory 11 and executed by the processor 12 to complete the present application. One or more modules can be a series of computer program instruction segments that can perform specific functions, and the instruction segments are used to describe the execution process of the computer program in the electronic device 10. For example, the computer program can be divided into a data acquisition module 100, a first calculation module 200, a second calculation module 300, a third calculation module 400, an external parameter optimization module 500, etc.

[0117] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the present invention. Anyone skilled in the art may modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by one of ordinary skill in the art without departing from the spirit and technical principles disclosed herein are intended to be covered by the claims of the present invention.

Claims

1. A method for optimizing sensor parameters of heavy-duty equipment, characterized in that: include: Collect sensor data of the heavy-duty equipment through sensors, and collect positioning information of the heavy-duty equipment through a preset navigation system; the sensors include cameras, laser radars, and inertial measurement devices; the sensor data include target image data collected by each camera, target point cloud data collected by each laser radar, and motion data collected by the inertial measurement device; Calculating a camera projection error of the camera based on target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a laser radar associated with the camera; Calculating the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars; Calculating a motion data error of the inertial measurement device based on the motion data and the positioning information; According to the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold, it is determined whether the external parameters of the camera and the external parameters of the lidar need to be optimized.

2. The method for optimizing sensor parameters of heavy-load equipment according to claim 1, characterized in that: The step of calculating the camera projection error of the camera based on the target image data, internal parameters, external parameters of the camera, and target point cloud data of a laser radar associated with the camera includes: Calculating projection image data based on the intrinsic and extrinsic parameters of the camera and target point cloud data of a laser radar associated with the camera; A camera projection error of the camera is calculated based on the target image data and the projection image data of the camera.

3. The method for optimizing sensor parameters of heavy-load equipment according to claim 1, characterized in that: The step of calculating the point cloud matching error of the laser radar based on the target point cloud data, external parameters, and target point cloud data of other laser radars includes: Calculate the product of the external parameter of the laser radar and the target point cloud data of other laser radars; The point cloud matching error of the laser radar is calculated based on the target point cloud data of the laser radar and the product.

4. The method for optimizing sensor parameters of heavy-load equipment according to claim 1, characterized in that: The step of calculating the motion data error of the inertial measurement device based on the motion data and the positioning information includes: The motion data error of the inertial measurement device is calculated according to the positioning information and the position information in the motion data.

5. The method for optimizing sensor parameters of heavy-load equipment according to claim 1, characterized in that: The step of determining whether it is necessary to optimize the camera's extrinsic parameters and the lidar's extrinsic parameters based on a comparison result of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold includes: Determine the matching error of each point cloud and the corresponding threshold, the projection error of each camera and the corresponding threshold, and the motion data error and the corresponding threshold: When all point cloud matching errors are less than the corresponding threshold, all camera projection errors are less than the corresponding threshold, and the motion data error is less than the corresponding threshold, there is no need to optimize the extrinsic parameters of the camera and the extrinsic parameters of the lidar; Otherwise, the external parameters of the camera and the external parameters of the lidar are optimized.

6. The method for optimizing sensor parameters of heavy-load equipment according to claim 5, characterized in that: The step of optimizing the external parameters of the camera and the external parameters of the laser radar includes: Based on a preset objective function, a total residual is calculated according to the point cloud data error, the camera projection error, and the motion data error; the objective function represents a functional relationship between the point cloud data error, the camera projection error, the motion data error, and the total residual; Solving the objective function, and when the objective function reaches a minimum value, obtaining updated data of the corresponding camera's extrinsic parameters and updated data of the lidar's extrinsic parameters; The external parameters of the camera are optimized according to the updated data of the external parameters of the camera, and the external parameters of the laser radar are optimized according to the updated data of the external parameters of the laser radar.

7. The method for optimizing sensor parameters of heavy-load equipment according to claim 1, characterized in that: Before the steps of collecting sensor data of the heavy-load equipment through the sensor and collecting positioning information of the heavy-load equipment through the preset navigation system, the sensor is also preliminarily optimized; The steps of the preliminary optimization include: The corresponding initial image data collected by each camera, the corresponding initial point cloud data collected by each lidar, and the initial motion data collected by the inertial measurement device; Processing the initial image data and the initial point cloud data respectively according to the initial motion data, and calculating the corresponding image delay duration and point cloud delay duration; Perform preliminary optimization processing on the corresponding camera according to the delay time of each image to obtain a preliminary optimized camera; The corresponding lidar is preliminarily optimized according to the delay time of each point cloud to obtain a preliminarily optimized lidar.

8. A device for optimizing sensor parameters of heavy-load equipment, characterized in that: include: A data acquisition module is used to collect sensor data of heavy-duty equipment and to collect positioning information of the heavy-duty equipment; The sensor data includes target image data collected by each camera, target point cloud data collected by each lidar, and motion data collected by the inertial measurement device; a first calculation module, configured to calculate a camera projection error of the camera based on target image data, intrinsic parameters, extrinsic parameters of the camera, and target point cloud data of a laser radar associated with the camera; A second calculation module is used to calculate the point cloud matching error of the laser radar based on the target point cloud data and external parameters of the laser radar and the target point cloud data of other laser radars; a third calculation module, configured to calculate a motion data error of the inertial measurement device based on the motion data and the positioning information; The external parameter optimization module is used to determine whether it is necessary to optimize the external parameters of the camera and the external parameters of the lidar based on the comparison results of the point cloud matching error and the corresponding threshold, the camera projection error and the corresponding threshold, and the motion data error and the corresponding threshold.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the method for optimizing sensor parameters of heavy-load equipment according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method for optimizing sensor parameters of heavy-load equipment according to any one of claims 1 to 7 are implemented.

Images