Method and device for positioning and mapping industrial automobile crane based on multi-sensing fusion

Abstract

The invention discloses an industrial automobile crane positioning and mapping method and device based on multi-sensing fusion, and relates to the field of machine sensing, and the method comprises the following steps: initializing a pose and offset of an inertial measurement unit by using an input laser point cloud and a pre-integration result of the inertial measurement unit; correcting laser point cloud motion distortion by using an inertial measurement unit, extracting a dynamic object point cloud, and synchronously extracting laser features and visual features with depth; calculating a key frame pose in real time by minimizing a visual re-projection error and an inertial measurement unit pre-integration error, and performing frame-to-map matching by taking the key frame pose as an initial value to further optimize the pose; a visual word bag model is utilized to quickly retrieve a closed-loop candidate frame, after laser passes geometric matching to verify a closed loop, a laser radar factor, a visual factor, an inertial measurement unit factor and a closed-loop factor are fused, a global factor graph is constructed and optimized, and a pose and point cloud map is updated. According to the method, the environment sensing capability of the industrial automobile crane can be improved, and the sensing accuracy and robustness are improved.

Description

Technical Field

[0001] This invention relates to the field of machine perception, and in particular to a method and apparatus for positioning and mapping of industrial truck cranes based on multi-sensor fusion. Background Technology

[0002] Industrial truck cranes combine the flexibility of a mobile platform with heavy lifting capabilities, making them key equipment in port logistics, infrastructure construction, and other scenarios, and significantly improving the efficiency of large cargo transfers. However, as a large mobile device with a dynamically changing operating space and sensors susceptible to interference from metal structures, its precise positioning and environmental conditions are crucial. Figure One Accurate pose perception is a persistent technological bottleneck in the industry. It is not only fundamental to safe hoisting operations but also a core prerequisite for collision avoidance and optimal path planning. Traditional single-sensor positioning solutions have significant limitations: laser point cloud distortion occurs in metal-reflective environments, and the robustness of vision drops sharply in low-light / smoky scenes. When cranes are in complex working conditions (such as narrow construction sites or dynamic interference environments), the accuracy and stability of existing positioning systems deteriorate drastically. To address the complementary needs of multimodal sensors, multi-sensor fusion technology offers a new solution. By collaboratively processing heterogeneous data sources, it constructs a redundant sensing system, theoretically overcoming the physical limitations of single sensors. However, the unique mechanical vibration characteristics of industrial truck cranes, the interference of metal structures on electromagnetic signals, and the computational load of large-scale scenes lead to problems such as accumulated spatiotemporal alignment errors, insufficient real-time performance, and mismapping of dynamic objects when directly applying standard fusion algorithms, making it difficult to meet the high safety standards required for engineering projects. To resolve these contradictions, a new positioning and mapping solution is urgently needed that can both leverage the advantages of multiple sensors to improve robustness and adapt to the real-time and accuracy requirements of harsh industrial environments. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention proposes a positioning and mapping method and device for industrial truck cranes based on multi-sensor fusion, and the technical solution adopted is as follows: According to a first aspect of the present invention, a method for positioning and mapping an industrial truck crane based on multi-sensor fusion is provided, the method comprising the following steps: S100 initializes the pose and the bias of the inertial measurement unit using the input laser point cloud and the pre-integration result of the inertial measurement unit. The S200 uses an inertial measurement unit to correct the motion distortion of laser point clouds, extracts dynamic object point clouds, and simultaneously extracts laser features and visual features with depth. The S300 calculates keyframe pose in real time by minimizing visual reprojection error and inertial measurement unit pre-integration error, and uses this as an initial value for frame-to-map matching to further optimize the pose. S400, a visual bag-of-words model is used to quickly retrieve the closed-loop candidate frame, after the laser is verified by geometric matching, the laser radar factor, the visual factor, the inertial measurement unit factor and the closed-loop factor are fused, the global factor graph is constructed and optimized, and the pose and the point cloud map are updated.

[0004] Further, the pose initialization in step S100 and the bias of the inertial measurement unit (IMU) satisfy the following conditions: First, define the world coordinate system as {W}, define the crane coordinate system as {B}, and assume that the inertial measurement unit (IMU) coordinate system is consistent with the crane coordinate system, then the state of the crane Can be written as Wherein is a rotation matrix, is a position vector, is a velocity, is an IMU bias, and the transformation from {B} to {W} Is expressed as ; The angular velocity and linear acceleration inputs of the IMU are defined as follows:

[0005] Wherein, And are the original IMU measurement values in {B} at time t, And are affected by slowly changing biases And white noise , is a rotation matrix from {W} to {B}, is a constant gravity vector in {W}; Next, use the measurement values of the IMU to deduce the motion of the crane, the velocity, position and rotation of the crane at time Can be calculated as follows:

[0006] Wherein, And assume that the angular velocity and linear acceleration of {B} remain constant during the above integration; then, the relative motion of the crane between two time steps is obtained by IMU pre-integration, and the IMU pre-integration measurement values between time i and j are calculated by the following formula , And :

[0007] Further, the step S200 of the present application corrects the motion distortion of the laser point cloud, extracts the dynamic object point cloud, corrects the motion distortion of the point cloud, and synchronously extracts the laser features and visual features with depth, and meets the following conditions: Within a single-frame laser radar scanning time window, the high-frequency angular velocity and linear acceleration data of the IMU are used to calculate the pose change in continuous time through pre-integration, and the continuous pose sequence within the start and end time of the laser radar scanning is obtained Then, an accurate timestamp is attached to each point in the laser point cloud, which is mapped from the acquisition time to the scanning start time, so that the correction of the motion distortion of the laser point cloud can be realized,

[0008] Among them, is the start time of the single-frame laser radar scanning, is the acquisition time of the i-th laser point, is the pose transformation from to t is the integral time variable, is the original laser point coordinate, is the corrected laser point coordinate; The corrected laser point cloud is projected onto the image plane to establish the correspondence between the laser point cloud and the image pixel coordinates, the pixel classification of the camera image is performed through the deep neural network, and the semantic probability map is output , C is the number of categories, each projection point is given a category label, and the point cloud marked as a dynamic category constitutes a candidate dynamic point set In the next k frames, the world coordinate sequence is calculated , and the motion residual is calculated , the points greater than the speed threshold are marked as dynamic points , avoiding the misclassification of static objects, and in subsequent positioning and mapping, the dynamic object point cloud is removed and a dynamic object layer is established separately; The point cloud is grouped according to the laser beam, and then the curvature of the points on each beam is calculated, and the curvature calculation formula is as follows:

[0009] Among them, is the curvature value of point i, is the neighborhood point set of point i, is the 3D coordinate vector of point i ), is the 3D coordinate vector of neighborhood point j; after calculating the curvature, the points greater than the curvature threshold and the points less than the curvature threshold are defined as corner points and plane points respectively; Visual feature point detection and tracking satisfy:

[0010]

[0011] in, Image gradient matrix eigenvalues, Let be the gradient of the pixel in the x / y direction. The calculated response value is considered a corner point if it is greater than the corner response threshold. Let be the pixel intensity at coordinate (x, y) at time t. The optical flow vector to be solved using Newton's iteration method; The depth values ​​of visual features are obtained directly from the LiDAR point cloud:

[0012]

[0013] in, These are the 3D coordinates of the laser point after correction in the lidar coordinate system. This is the transformation matrix from the lidar coordinate system to the camera coordinate system. For the laser point projected onto the 3D point in the camera coordinate system, This represents the depth value of the feature point.

[0014] Furthermore, the pose optimization process in step S300 of the present invention satisfies the following conditions: First, laser point cloud projection is used to provide accurate depth values ​​for visual feature points, constructing 3D feature points with depth. Then, tight coupling optimization is performed on keyframes to minimize visual reprojection error. and IMU pre-integration error The objective function is Finally, the pose at any time is calculated in real time between keyframes using IMU pre-integration interpolation. Using this pose as the initial estimate, the pose is optimized through scan-to-map matching: a local point cloud map is constructed, and the corner points of the current frame are extracted; geometric constraint residuals are established, where corner point matching uses the distance from a point to a line, and planar point matching uses the distance from a point to a plane; the residual function is minimized by solving the LM algorithm, combined with KD-Tree to accelerate the search and multi-threaded residual calculation, thereby achieving pose optimization.

[0015] Furthermore, the global factor graph construction and optimization process in step S400 of the present invention satisfies the following conditions: Global optimization is achieved through tightly coupled multi-sensor factor graphs: when a new keyframe is inserted or loop closure detection is triggered, a graph containing the poses of all keyframes is constructed. a factor graph of LIS, adding four kinds of constraint factors: laser inertial navigation system factor wherein is the relative pose estimated by LIS; visual inertial system factor wherein is the relative pose estimated by VIS; IMU pre-integration factor ; loop closure factor wherein is calculated by loop closure matching; finally, the objective function is solved by iSAM2 optimizer achieve global consistent optimization of pose and update global point cloud map, wherein, is the covariance matrix of each factor. Finally, the dynamic layer and the global static point cloud map obtained above are registered by matching the depth information of the matched image and the depth information of the laser point cloud, and a hybrid map is constructed.

[0016] According to the second aspect of the application, a positioning and mapping device for industrial truck cranes based on multi-sensor fusion is provided, the device comprising: a system initialization module for initializing the pose and the bias of the inertial measurement unit according to the input laser point cloud and the result of the inertial measurement unit pre-integration; a feature extraction module for correcting the motion distortion of the laser point cloud, extracting the dynamic object point cloud and synchronously extracting the laser features and visual features with depth; a local odometry module for calculating the key frame pose in real time by minimizing the visual re-projection error and the inertial measurement unit pre-integration error, and further optimizing the pose by frame-to-map matching with the initial value; a global factor graph optimization module for verifying the loop closure, fusing the laser radar factor, the visual factor, the inertial measurement unit factor and the loop closure factor, constructing and optimizing the global factor graph, and updating the pose and the point cloud map.

[0017] The application has at least the following beneficial effects: The positioning and mapping method and device for industrial truck cranes based on multi-sensor fusion provided by the application make full use of the advantages of multi-sensor information fusion. On the one hand, a laser-visual-inertial tightly coupled perception chain is constructed, the dominant sensor is dynamically switched in the degenerative scene such as metal interference and weak light through time and space synchronous calibration and adaptive weight distribution, and the continuity of positioning is ensured; on the other hand, a hierarchical optimization architecture is adopted, a front-end lightweight odometry realizes high-frequency pose output, and a rear-end global factor graph optimization ensures global consistency, and the calculation delay of large-scale scene is significantly reduced. The method is adapted to different tonnage cranes and complex construction site environments through modular sensor interface and online calibration mechanism, has good applicability and scalability.

[0018] It is to be understood that the details described in this section are not intended to identify key or critical elements of the embodiments of the application or to limit the scope of the application. Other features of the present application will become apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative effort on the basis of these drawings.

[0020] Figure 1 A flow chart of a positioning and mapping method of an industrial truck crane based on multi-sensor fusion provided by the embodiments of the present application is shown in the figure. Figure 2 A design drawing of a positioning and mapping device of an industrial truck crane based on multi-sensor fusion provided by the embodiments of the present application is shown in the figure. In the figure: 1, laser radar and data box thereof; 2, IMU inertial measurement unit; 3, binocular depth camera; 4, NUC mini computer; 5, base and telescopic clamp.

[0021] Figure 3 A test effect drawing of the positioning and mapping method of the industrial truck crane based on multi-sensor fusion provided by the embodiments of the present application in an actual scene is shown in the figure. Figure 4 A composition block diagram of the positioning and mapping device of the industrial truck crane based on multi-sensor fusion provided by the embodiments of the present application is shown in the figure. DETAILED DESCRIPTION

[0022] The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the drawings of the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative effort fall within the scope of the present application.

[0023] The embodiments of the present application provide a positioning and mapping method of an industrial truck crane based on multi-sensor fusion, as shown in the figure, the method comprises the following steps: Figure 1 S100, initializing the pose and the bias of the inertial measurement unit by using the input laser point cloud and the pre-integrated result of the inertial measurement unit.

[0024] ​In the examples of the present application, due to the long boom of the industrial truck crane and the large operating space range, compared with the ground mobile robot and the unmanned aerial vehicle, the industrial truck crane has a larger operating space in the vertical direction. In order to realize panoramic perception, the present application sets up a perception suite and enables the perception suite to have panoramic perception capability, see the accompanying Figure 2 The perception suite set up by the present application includes a laser radar for realizing high-precision three-dimensional space modeling, generating point cloud data of the environment, and storing the generated point cloud data in a data box, the laser radar being capable of working under different light conditions; a depth camera for acquiring image information and semantic information, which is helpful for removing dynamic objects and improving the robustness of mapping; an inertial measurement unit (IMU) for providing real-time acceleration and angular velocity data for pose estimation, which can help improve the accuracy of other sensor data in a dynamic environment; and a NUC mini computer for operating the data provided by the laser radar, the depth camera and the inertial measurement unit (IMU) and transmitting the operation results. The bottom of the perception suite is provided with a base and a telescopic clamp, and the laser radar, the depth camera, the inertial measurement unit (IMU) and the NUC mini computer are fixedly installed on the base and are integrally installed on the industrial truck crane through the telescopic clamp. Specifically, the perception suite can be installed above the cockpit.

[0025] In the examples of the present application, the initialization pose and the bias of the inertial measurement unit satisfy the following conditions: the world coordinate system is defined as {W}, the crane coordinate system is defined as {B}, and it is assumed that the inertial measurement unit (IMU) coordinate system is consistent with the crane coordinate system, then the state of the crane can be written as where R is a rotation matrix, is a position vector, is a velocity, is an IMU bias, and the transformation from {B} to {W} is represented as . .

[0026] The angular velocity and linear acceleration inputs of the IMU are defined as follows:

[0027] where and are the raw IMU measurements in {B} at time t. and are affected by a slowly changing bias and white noise . is the rotation matrix from {W} to {B}, is the constant gravity vector in {W}.

[0028] Next, the measurements of the IMU are used to infer the motion of the crane. The velocity, position and rotation of the crane at time can be calculated as follows:

[0029] where and the angular velocity and linear acceleration of {B} are assumed to be constant during the above integration. Then, the relative motion of the crane between two time steps can be obtained by IMU pre-integration, and the IMU pre-integrated measurements between times i and j can be calculated using the following equation , and :

[0030] S200, correcting the laser point cloud motion distortion by using the inertial measurement unit, extracting the dynamic object point cloud and synchronously extracting the laser features with depth and visual features.

[0031] In the examples of the present application, the laser point cloud motion distortion is corrected by IMU pre-integration: within the start and end time window of each frame of laser scanning, a continuous pose sequence is constructed using high-frequency IMU data. For each laser point in the point cloud carrying an accurate timestamp, the pose transformation of its acquisition time relative to the start time of the scan is calculated by linear interpolation, and the point is back-projected back to the starting coordinate system, so that the corrected point cloud restores the true geometric shape.

[0032] In the examples of the present application, there are dynamic information such as workers and vehicles carrying goods in the working scene of the industrial truck crane, and the dynamic object information can be easily extracted by a deep neural network to construct a dynamic layer map. The high-altitude area above the cockpit is relatively empty, the observation distance near the hook is moderate, and there is no blind area, and by identifying the hook and the transported goods, a target layer map can be constructed. Since the amount of information extracted is huge, in order to enable the system to run in real time, a knowledge distillation compressed neural network is used, and high-level image information is extracted.

[0033] In the examples of the present application, within the single-frame laser radar scanning time window, the continuous pose sequence is obtained by pre-integrating the high-frequency angular velocity and linear acceleration data of the IMU to calculate the pose change in continuous time.

[0034]

[0035] where is the start time of single-frame laser radar scanning, is the acquisition time of the i-th laser point, is the acquisition time of the i-th laser point, is the acquisition time of the i-th laser point, is the pose transformation of the i-th laser point, is the original laser point coordinate, is the corrected laser point coordinate.

[0036] The depth value of the visual feature can be directly obtained from the laser radar point cloud:

[0037]

[0038] wherein, is the 3D coordinate of the corrected laser point in the laser radar coordinate system, is the transformation matrix from the laser radar coordinate system to the camera coordinate system, is the 3D point of the laser point projected to the camera coordinate system, is the depth value of the feature point.

[0039] The corrected laser point cloud is projected to the image plane to establish the correspondence between the laser point cloud and the image pixel coordinate, the pixel of the camera image is classified by a depth neural network, and a semantic probability map is output (C is the number of categories), each projected point is assigned a category label, and the point cloud marked as a dynamic category constitutes a candidate dynamic point set , and in the next k frames, the world coordinate sequence of the point is calculated , and the motion residual is calculated , and the points greater than the speed threshold are marked as dynamic points to avoid misclassification of static objects (such as stationary vehicles, staff, etc.). In subsequent positioning and mapping, the dynamic object point cloud is removed and a separate dynamic object layer is established.

[0040] In the present example, a lightweight screening strategy based on curvature is adopted: first, the point cloud is grouped according to the scanning line, and the neighborhood curvature of each point on each line is calculated (taking 5 points on the left and right of the scanning line). The curvature calculation formula essentially measures the local surface concave-convexity - the curvature of a flat area is close to 0, and the curvature of an edge corner increases sharply. The system sets two thresholds: a high threshold (such as 0.1) to screen corner points (edge features such as wall edges), and a low threshold (such as 0.01) to screen plane points (plane features such as floors and walls). To avoid feature accumulation, only the 2 largest corner points and the 4 smallest plane points on each scanning line are retained, and adjacent redundant points are filtered by non-maximum suppression.

[0041] In the present embodiment, the point cloud is grouped by laser beam, and then the curvature of the points on each beam is calculated, and the curvature calculation formula is as follows:

[0042] wherein, is the curvature value of point i, is the neighborhood point set of point i, is the 3D coordinate vector of point i, is the 3D coordinate vector of neighborhood point j. After calculating the curvature, the points greater than the curvature threshold and the points less than the curvature threshold are defined as the corner points and the plane points respectively.

[0043] In the present embodiment, the image key points are extracted by Shi-Tomasi corner detector, and the feature point trajectories of adjacent frames are tracked by KLT optical flow. The traditional visual odometry needs to estimate the depth of feature points by multi-view triangulation, which is time-consuming and susceptible to texture loss. Then, the corrected laser point cloud is projected to the camera coordinate system, and the nearest laser point depth value is assigned to each visual feature point. For example, the detected shelf corner point in the image directly obtains the accurate three-dimensional coordinates by searching for the laser points around it.

[0044] In the present embodiment, the visual feature point detection and tracking satisfy:

[0045]

[0046] wherein, is the eigenvalue of the image gradient matrix is the gradient of the pixel point in the x / y direction. is the calculated response value, and when it is greater than the corner response threshold, the feature point is considered as a corner point. is the pixel intensity of the coordinate (x, y) at time t, is the optical flow vector to be solved by Newton iteration method.

[0047] S300, the key frame pose is calculated in real time by minimizing the visual re-projection error and the inertial measurement unit pre-integration error, and the pose is further optimized by frame-to-map matching with the initial value.

[0048] ​​In the present example, the visual-inertial system incorporating IMU acquires the correspondence of 2D feature points between adjacent frames by feature tracking. Unlike traditional visual odometry, the present application directly provides accurate depth values for feature points by laser point cloud projection, and constructs a 3D feature point set with accurate scale. Based on this, the system constructs a local nonlinear optimization problem, and the objective function includes a visual re-projection error term and an IMU pre-integration error term. The visual re-projection error is defined as the position deviation of the feature points from the world coordinate system to the image plane of the current frame; the IMU pre-integration error forcibly constrains the consistency of rotation, velocity and displacement between adjacent key frames. The optimization process is triggered when the key frame is selected, and the nonlinear optimization algorithm is used to simultaneously solve the key frame pose, motion velocity and IMU sensor zero offset, and the optimized key frame state estimation is output.

[0049] In the present example, in the time interval of non-key frames, the system realizes high-frequency pose output by using the physical characteristics of the IMU. Taking the latest key frame pose as the reference, combining the real-time measured angular velocity and acceleration data of the IMU, the relative motion transformation at any time relative to the key frame is calculated by the pre-integration model. The current time pose is directly obtained by the product of the key frame pose and the pre-integration transformation.

[0050] In the present example, the high-frequency pose output by the visual-inertial system is input as the initial estimation into the laser positioning module. The laser-inertial system incorporating the laser positioning module dynamically maintains a local point cloud map, which contains the edge features (corner points) and plane features (plane points) extracted from the historical key frames. For the current laser frame, first, the feature points are converted to the map coordinate system according to the initial pose, and then two types of geometric constraints, edge feature matching and plane feature matching, are established, and the objective function is defined as the weighted sum of the distance residuals, and the optimal pose is solved by the iterative optimization algorithm.

[0051] In the present example, first, the laser point cloud projection is used to provide accurate depth values for visual feature points, and a 3D feature point with depth is constructed, then tight coupling optimization is performed on the key frame, and the visual re-projection error and the IMU pre-integration error are minimized, the objective function is , and finally the pose at any time is calculated in real time by IMU pre-integration interpolation between key frames. Taking the pose as the initial estimation, the pose is optimized by scan-to-map matching: a local point cloud map is constructed, and the corner points of the current frame are extracted; geometric constraint residuals (point-to-line distance for corner point matching, and point-to-plane distance for plane point matching) are established; the LM algorithm is used to solve the minimum residual function, and the KD-Tree acceleration search and multi-thread residual calculation are combined, so as to realize the optimization of the pose.

[0052] S400, the visual bag-of-words model is used for quickly searching a closed loop candidate frame, after laser is verified through geometric matching, laser radar factors, visual factors, inertial measurement unit factors and closed loop factors are fused, a global factor graph is constructed and optimized, a pose and a point cloud map are updated.

[0053] In the examples of the present application, the system performs efficient closed loop candidate search on the current key frame through the visual bag-of-words model. The model converts the key frame image features into a compact vector representation based on an offline trained visual dictionary, quickly matches similar scenes in the historical key frame database through an inverted index, and outputs Top-K candidate frames. Since visual similarity may be affected by changes in viewing angle or dynamic objects, the system introduces laser geometric verification as a secondary check: the local point cloud map of the candidate frame is iteratively closest point matched with the current frame point cloud, a rigid transformation matrix between the two frames is calculated, and a matching residual is calculated. When the registration residual is lower than a threshold (such as the average point-to-plane distance <0.1m) and the spatial distance consistency passes the chi-square test, it is determined that the closed loop is established.

[0054] In the examples of the present application, the verified closed loop event triggers the reconstruction of the global factor graph. The system integrates four types of sensor constraint factors: laser odometry factors (relative pose constraints constructed based on continuous key frame laser matching results), visual odometry factors (inter-frame motion estimation based on visual re-projection optimization), IMU pre-integration factors (encoding the inertial kinematics constraints of adjacent key frames), and closed loop factors (representing the spatial pose relationship between the verified closed loop frame pairs), optimizes the pose state of all historical key frames, solves through the incremental smoothing algorithm iSAM2, and realizes online correction of the full trajectory pose.

[0055] In the examples of the present application, after the pose optimization is completed, the system performs synchronous update of the map. First, according to the optimized key frame pose, the associated original point cloud is re-projected into the world coordinate system. Then, based on multi-frame observation consistency, dynamic objects (such as moving vehicles) are detected, and their point clouds are excluded from the static map. Next, according to the spatial distribution density, the key frame set is thinned out, and frames with a pose distance greater than 0.5m are retained to control the size of the map. Finally, a voxel filter with a resolution of 0.1m is used to downsample the global map, balancing detail retention and storage efficiency.

[0056] In the examples of the present application, global optimization is achieved through tightly coupled multi-sensor factor graphs: when a new key frame is inserted or closed loop detection is triggered, a factor graph containing all key frame poses is constructed, and four types of constraint factors are added: laser inertial navigation system factors (where is the relative pose estimated by LIS); visual inertial navigation system factors (where is the relative pose estimated by VIS); IMU pre-integration factors ; closed loop factor (wherein calculated by closed loop matching). Finally, the objective function is solved by the iSAM2 optimizer achieves global consistent optimization of poses and updates the global point cloud map. Wherein, covariance matrix of each factor. Finally, the dynamic layer and the global static point cloud map obtained above are registered by matching the image depth information and the laser point cloud depth information, and a hybrid map is constructed.

[0057] The positioning and mapping method for the industrial truck crane based on multi-sensor fusion provided by the embodiment of the application has the positioning and mapping effect as shown in Figure 3 The point cloud map accurately reconstructs the three-dimensional space structure of the debugging field, and verifies the superiority of multi-sensor fusion in complex industrial scenes.

[0058] Based on the same inventive concept, the embodiment of the application also provides a positioning and mapping device for an industrial truck crane based on multi-sensor fusion, referring to the accompanying Figure 4 , the device comprises: A system initialization module is configured to initialize the pose and the bias of the inertial measurement unit according to the input laser point cloud and the pre-integration result of the inertial measurement unit. A feature extraction module is configured to correct the motion distortion of the laser point cloud, extract the dynamic object point cloud, and simultaneously extract the laser features and visual features with depth. A local odometry module is configured to calculate the key frame pose in real time by minimizing the visual re-projection error and the inertial measurement unit pre-integration error, and further optimize the pose by frame-to-map matching with the calculated key frame pose as the initial value. A global factor graph optimization module is configured to verify the closed loop, fuse the lidar factor, the visual factor, the inertial measurement unit factor and the closed loop factor, construct and optimize the global factor graph, and update the pose and the point cloud map.

[0059] The device can be used to perform the method shown in the embodiment shown in Figure 1 , therefore, the functions that can be achieved by each functional module of the device can refer to the description of the embodiment shown in Figure 1 , and will not be described in detail.

[0060] It should be understood that the steps shown above can be reordered, added or deleted. For example, each step described in the present application can be executed in parallel, sequentially or in a different order, as long as the desired results of the technical solutions disclosed in the present application can be achieved, and the present application does not limit this.

[0061] The above detailed description does not limit the scope of the application. Various modifications, combinations, sub-combinations and alternatives can be made to the detailed embodiment within the scope of the application. Any modification, equivalent replacement and improvement made without departing from the spirit and principle of the application shall fall within the scope of the application.

Claims

1. A positioning and mapping method for industrial truck cranes based on multi-sensory fusion, characterized in that, The method comprises the following steps: S100, initializing a pose and a bias of an inertial measurement unit (IMU) by using an input laser point cloud and a result of pre-integration of the IMU; S200, correcting motion distortion of the laser point cloud by using the IMU, extracting a dynamic object point cloud, and synchronously extracting laser features with depth and visual features; S300, calculating a key frame pose in real time by minimizing visual re-projection errors and IMU pre-integration errors, and further optimizing the pose by frame-to-map matching with the key frame pose as an initial value; S400, quickly searching for a closed-loop candidate frame by using a visual bag-of-words model, verifying the closed loop by using laser geometry matching, fusing a laser radar factor, a visual factor, an IMU factor, and a closed-loop factor, constructing and optimizing a global factor graph, and updating the pose and a point cloud map.

2. The method of claim 1, wherein, The pose initialization and the bias of the IMU in step S100 satisfy the following conditions: First, define the world coordinate system as {W}, define the crane coordinate system as {B}, and assume that the inertial measurement unit (IMU) coordinate system is consistent with the crane coordinate system, then the state of the crane where is a rotation matrix, is a position vector, is a velocity, is an IMU bias, and the transformation from {B} to {W} is represented as ;​ The input definitions of angular velocity and linear acceleration of the IMU are as follows: where and are the original IMU measurements in {B} at time t, and are affected by slowly changing biases and white noise , is the rotation matrix from {W} to {B}, is the constant gravity vector in {W}; Next, the measurements of the IMU are used to infer the motion of the crane, which is at time The velocity, position and rotation calculations are as follows: wherein and assuming that the angular velocity and linear acceleration of {B} remain constant during the above integration; Then, the relative motion of the crane between two time steps is obtained by IMU pre-integration, which is calculated using the following equation for the IMU pre-integration measurements between times i and j , and : 。 3. The method of claim 1, wherein, The correction of the motion distortion of the laser point cloud, the extraction of the dynamic object point cloud, and the synchronous extraction of the laser features with depth and the visual features in step S200 satisfy the following conditions: In the single-frame laser radar scanning time window, the high-frequency angular velocity and linear acceleration data of the IMU are used to calculate the pose change in continuous time through pre-integration, and the continuous pose sequence in the start and end time of the laser radar scanning is obtained Then, the accurate timestamp of each point in the laser point cloud is attached, which is mapped from the acquisition time to the scanning start time, so that the correction of the motion distortion of the laser point cloud can be realized. wherein, is the single-frame laser radar scanning start time, is the i-th laser point acquisition time, is to pose transformation, t is the integral time variable, is the original laser point coordinate, is the corrected laser point coordinate; The corrected laser point cloud is projected to an image plane to establish a corresponding relationship between the laser point cloud and image pixel coordinates, a camera image is classified by a deep neural network, and a semantic probability map is output , C is a category number, a category label is given to each projection point , and a point cloud marked as a dynamic category constitutes a candidate dynamic point set , a world coordinate sequence of the point is calculated in continuous k frames , a motion residual is calculated again , and a point greater than a speed threshold is marked as a dynamic point , misclassification of a static object is avoided, and in subsequent positioning and mapping, dynamic object point clouds are removed and a dynamic object layer is separately established; The point cloud is grouped according to laser beams, and then the curvature of the points on each beam is calculated, and the curvature calculation formula is as follows: in, Let i be the curvature value at point i. Let i be the set of neighborhood points. Let i be the 3D coordinate vector of point i. ), Let be the 3D coordinate vector of the neighborhood point j; after calculating the curvature, points greater than the curvature threshold and points less than the curvature threshold are defined as corner points and planar points, respectively; The visual feature point detection and tracking satisfy the following conditions: wherein, is an eigenvalue of the image gradient matrix , is a gradient of the pixel point in x / y direction, is a calculated response value, when it is greater than a corner response threshold value, it is considered that the feature point is a corner, is a pixel intensity of the coordinate (x, y) at time t, is an optical flow vector to be solved by Newton iteration method; The depth value of the visual feature is directly obtained from the laser radar point cloud: wherein, is the 3D coordinate of the corrected laser point in the LiDAR coordinate system, is the transformation matrix from the LiDAR coordinate system to the camera coordinate system, is the 3D point of the laser point projected to the camera coordinate system, is the depth value of the feature point.

4. The method of claim 1, wherein, The pose optimization process in step S300 satisfies the following conditions: Firstly, the laser point cloud is projected to provide accurate depth values for visual feature points, and 3D feature points with depth are constructed. Then, tight coupling optimization is performed on the key frames to minimize the visual re-projection error and IMU pre-integration error , the objective function is Finally, the pose at any time is calculated in real time by IMU pre-integration interpolation between key frames, and the pose is optimized by scan-to-map matching with the initial estimate: a local point cloud map is constructed, and the corner points of the current frame are extracted; geometric constraint residuals are established, in which the point-to-line distance is used for corner point matching, and the point-to-plane distance is used for plane point matching; the least squares function is solved by the LM algorithm, and the KD-Tree acceleration search and multi-thread residual calculation are combined to realize the optimization of the pose.

5. The method of claim 1, wherein, The global factor graph construction and optimization process in step S400 satisfy the following conditions: Global optimization is achieved through tightly coupled multi-sensor factor graphs: when a new keyframe is inserted or loop closure detection is triggered, a graph containing the poses of all keyframes is constructed. The factor plot is modified by adding four constraint factors: laser inertial navigation system factor. ,in Relative pose estimated by LIS; Visual-Inertial System Factor ,in Relative pose estimated by VIS; IMU pre-integration factor Closed-loop factor ,in The calculation is performed using closed-loop matching; finally, the objective function is solved using the iSAM2 optimizer. Achieve globally consistent pose optimization and update the global point cloud map, whereby... Let be the covariance matrix of each factor.

6. A multi-sensory fusion based positioning and mapping device for industrial vehicle cranes, implementing the method of any of claims 1-5, characterized by, The device comprises: a system initialization module configured to initialize a pose and a bias of an inertial measurement unit (IMU) by using an input laser point cloud and a result of pre-integration of the IMU; a feature extraction module configured to correct motion distortion of the laser point cloud, extract a dynamic object point cloud, and synchronously extract laser features with depth and visual features; a local odometry module configured to calculate a key frame pose in real time by minimizing visual re-projection errors and IMU pre-integration errors, and further optimize the pose by frame-to-map matching with the key frame pose as an initial value; a global factor graph optimization module configured to verify a closed loop by using laser geometry matching, fuse a laser radar factor, a visual factor, an IMU factor, and a closed-loop factor, construct and optimize a global factor graph, and update the pose and a point cloud map.

Images