A method for repositioning of a smart bridge crane

Abstract

The application discloses a kind of repositioning methods for intelligent bridge crane, through real-time collection in the environment data of process by sensing system in solid-state laser radar, the original point cloud data collected by solid-state laser radar is converted into octree map format in real time to establish environment map and update, after building, octree map data is converted into point cloud data, the process will obtain the prior environment map of bridge crane;Then through the data of solid-state laser radar obtained, the position and attitude of bridge crane in prior environment map are estimated using rough matching based on point cloud descriptor by solid-state laser radar data.The initial pose of bridge crane is determined by the fusion of the fine matching and rough matching of ICP algorithm;After obtaining initial pose, the relative pose of bridge crane in map is estimated by combining prior environment map and observation data of solid-state laser radar, and the positioning result of bridge crane is realized by using the tight coupling fusion scheme of solid-state laser radar and IMU data.

Description

Technical Field

[0001] This invention relates to a repositioning method, specifically a repositioning method for intelligent bridge cranes, belonging to the field of intelligent bridge crane positioning technology. Background Technology

[0002] In the field of smart factories, the method of positioning bridge cranes based on encoders or single ranging sensors is low in cost and basically meets the requirements. However, the errors accumulated over a long period of time need to be eliminated manually. Since the elimination process is inconvenient and the bridge crane needs to be stopped when eliminating errors, the working efficiency of the bridge crane is reduced.

[0003] In the field of relocation, the current approach involves fusing solid-state LiDAR with IMU (Integrated Measurement Unit), utilizing pre-built prior point cloud maps and real-time sensor measurement data to design relocation algorithm modules, divided into global initial pose estimation and local localization. This method improves the accuracy and reliability of positioning without accumulating errors. In mobile robotics, autonomous driving, and smart factories, this relocation technology is crucial. Currently, domestic and international research on relocation technology primarily focuses on autonomous driving and mobile robotics, while its application to intelligent bridge cranes in workshops is rarely discussed. Compared to the environment where autonomous driving provides GPS and other positioning information, bridge cranes in workshops lack precise positioning information such as GPS and RTK (Real-Time Kinematics). Therefore, using existing relocation methods in relatively enclosed environments like factories results in low positioning accuracy and slow speed, leading to inefficiency. Thus, providing a new relocation method that effectively ensures the accuracy of bridge crane positioning while maintaining good positioning efficiency, thereby providing reliable positioning data for subsequent tasks, is one of the research directions in this industry. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a repositioning method for intelligent bridge cranes, which can effectively ensure the accuracy of bridge crane positioning and also ensure good positioning efficiency, thereby providing reliable positioning data for subsequent tasks in a timely manner.

[0005] To achieve the above objectives, the technical solution adopted by this invention is: a repositioning method for an intelligent bridge crane, wherein a sensing system is installed on the bridge crane, the sensing system including a solid-state lidar, an IMU sensor, and a computer, the computer including a timestamp synchronization module, a data fusion module, and a display; the solid-state lidar is used to collect point cloud images of the surrounding environment and transmit them to the computer; the IMU sensor is used to provide real-time spatial pose and acceleration information of the bridge crane and transmit it to the computer; the timestamp synchronization module in the computer is used to synchronize the clock of the solid-state lidar with the main clock of the computer; the data fusion module is used to fuse and analyze the data collected by the solid-state lidar and the data fed back by the IMU sensor, and then transmit the final real-time positioning result to the display for display. The specific repositioning steps are as follows:

[0006] A. First, calibrate the solid-state lidar coordinate system and the bridge crane coordinate system separately, and then transform the solid-state lidar coordinate system to the bridge crane coordinate system.

[0007] B. Synchronize the clock of the solid-state lidar with the main clock of the computer through the timestamp synchronization module;

[0008] C. After acquiring real-time point cloud data through solid-state LiDAR, an octree map is constructed. Then, the octree map is converted into a point cloud map and updated in real time to obtain the prior environment map in the relocalization process.

[0009] D. Based on the prior environment map obtained in step C, use the point cloud descriptor algorithm to perform coarse matching on the current solid-state lidar observation data, create the ScanContext matrix, and then estimate the position and attitude of the bridge crane in the prior environment map based on the ScanContext matrix.

[0010] E. A fusion scheme based on fine matching using the ICP algorithm and coarse matching based on point cloud descriptors in step D (this process can be implemented using known methods) is used to determine the initial pose of the bridge crane.

[0011] F. Based on the initial pose obtained in step E, combined with the prior environment map and the observation data of the lidar, estimate the relative pose of the bridge crane in the prior environment map, and obtain the positioning result of the bridge crane by using a tightly coupled fusion method of solid-state lidar and IMU.

[0012] Furthermore, the specific process of step A is as follows: When the bridge crane is moving, the real-time coordinates of the bridge crane can be obtained through the encoder on the bridge crane. By setting three calibration objects, two sets of calibration object coordinates corresponding to the solid-state lidar coordinate system and the bridge crane coordinate system are obtained respectively. The transformation relationship between the solid-state lidar coordinate system and the bridge crane coordinate system is obtained by using the two sets of calibration object coordinates, so as to realize the transformation from the solid-state lidar coordinate system to the bridge crane coordinate system.

[0013] Furthermore, in step B, the time synchronization process uses the IEEE 1588v2.0 PTP Delay request-response mechanism, with the Livox device acting as a slave to perform PTP time synchronization with the computer clock device.

[0014] Furthermore, the specific process of constructing the octree map in step C is as follows:

[0015] (1) Determine the number of octree layers N by the leaf nodes and the actual range of the laser point cloud being processed. max It must satisfy the formula:

[0016]

[0017] Where l0 is the size of the leaf node of the octagonal tree, l max This represents the maximum side length of the point cloud spatial range, i.e., the root node size.

[0018] (2) Determine the voxel encoding of the leaf node where each point is located in the laser point cloud data. First, define the octree encoding method, the specific formula of which is:

[0019]

[0020] Where, m a It is an octal number representing the index of the node among its eight sibling nodes at the same level. Similarly, m a+1 m a The parent node is the index of its eight sibling nodes at the same level. The above formula fully describes the path from each leaf node to the root node in the octree. Therefore, the position of each leaf node in the spatial model is determined by its octree encoding. It should be noted that point cloud data is only stored in the leaf nodes, and intermediate nodes are only used as path channels during data retrieval.

[0021] Assuming the three-dimensional coordinates of the laser point are P(x,y,z), the minimum coordinate value of the vertex of the largest cube represented by the root node of the octree is expressed as (x...y...z). min ,y min ,z minIf the coordinates are 3D, then the spatial index value Q(i,j,k) is obtained by calculation using the following formula:

[0022]

[0023] in, This indicates rounding down, converting the index value Q(i,j,k) to its binary representation:

[0024]

[0025] The octree encoding of the voxel containing the current laser point cloud data is then represented as:

[0026] m a =i a +j a 2 a +k a 2 2 , a∈{0,1,…,n-1}

[0027]

[0028] The number of leaf nodes at the bottom level of an octree structure is determined by the number of levels N. max This decision completes the construction of the octree map.

[0029] Furthermore, the specific process of creating the ScanContext matrix in step D is as follows:

[0030] (1) Point cloud spatial division

[0031] For a set P of laser point clouds, a secondary partitioning is performed along the radial and circumferential directions of the XOY plane. First, with the origin of the lidar coordinates as the center, the space is divided into N equidistant partitions along the direction of increasing radius. r Concentric rings; assuming the maximum sensing range of the lidar is L max Then the width of each ring is d. r :

[0032]

[0033] The second division involves cutting each ring into N equal-angled segments. s If the sector is a single sector, then the angle α of that single sector is... s for:

[0034]

[0035] Assumption Let represent the set of all laser points whose coordinate values ​​fall within the j-th sector region of the i-th annulus. Then, the set of laser point clouds in the segmented frame is represented as:

[0036]

[0037] (2) Generate Scan Context matrix

[0038] The point cloud segmented in the above manner is represented as an N r ×N s The matrix is ​​a series of points, where each row represents a ring in the actual space and each column corresponds to a sector within an angular range. Since the maximum height information of all 3D points within each sector is less affected by the environment, it effectively reflects the vertical shape characteristics of the point cloud structure within the region. Therefore, [the matrix is ​​used]. The maximum value of the Z-coordinate is taken as the characteristic value of this sector region, which is represented by the value φ of each element in the matrix as:

[0039]

[0040] Where z(p) represents Given the Z-coordinate of a point p within the surface, the Scan Context matrix I can be represented in the following form:

[0041]

[0042] If there are no points in the region, the feature value is set to 0;

[0043] (3) Similarity difference calculation

[0044] Definition I q For the Scan Context matrix of the point cloud data at the current time, I c Given the Scan Context matrix of point cloud data at a certain historical moment, then I q with I c The difference in similarity between them is defined as:

[0045]

[0046] In the formula, Indicate I q The j-th column in Indicate I c The j-th column in the array.

[0047] Furthermore, the specific process of step F is as follows:

[0048] I. Laser Odometry

[0049] ① First, select high-quality points based on four aspects: depth, refraction angle, intensity, and incident angle. The principles for eliminating badpoints are: a. points with a large refraction angle; b. points with excessively high or low intensity; c. points with excessively small incident angle; d. points that are blocked.

[0050] ② Feature extraction is performed using the curvature magnitude. Curvature characterizes the local surface smoothness of the laser point. The specific formula for calculating curvature is as follows:

[0051]

[0052] in, and This represents the coordinates of the i-th and j-th scan points on the l-th laser beam in the k-th frame of data; This indicates the nearest point to the target point and its neighboring points with the same laser beam number;

[0053] ③ Perform matching optimization, register the feature points scanned in the kth frame with the already constructed prior environment map, and obtain the pose transformation relationship of the current frame point cloud relative to the prior environment map, providing odometry output;

[0054] II. IMU Pre-integration Algorithm

[0055] Based on the rigid body kinematics model in three-dimensional space, the differential equation of the IMU motion model is as follows:

[0056]

[0057] in, This represents the time derivative of the displacement of the IMU coordinate system relative to the global coordinate system at time t. This represents the derivative of the velocity with respect to time in the global coordinate system at time t. Let t represent the derivative of the quaternion of the IMU coordinate system with respect to the global coordinate system at time t with respect to time. This represents quaternion multiplication; the attitude description uses quaternion differential equations, which can also be written in the form of differential equations for rotation matrices:

[0058]

[0059] in Represents the rotational angular velocity, i.e., the angular velocity output of the gyroscope in actual use; [·]× indicates an antisymmetric matrix.

[0060] Based on the equations of motion, we can obtain the continuous integral results of the IMU from time i to time j:

[0061]

[0062] Define the integral term in the above formula as:

[0063]

[0064] Since the interval between two adjacent samples is short, it is assumed that the zero bias of the IMU remains constant within one sampling period, i.e., the bias is equal. However, as a state variable, changes in the bias will also cause changes in the integral term. Performing a Taylor expansion of the pre-integrated result at the bias, the integral term will then take the following form:

[0065]

[0066] The Jacobian matrix is ​​specifically represented as follows: The pre-integral formula is then rearranged as follows:

[0067]

[0068] Substituting the above formula and rearranging, we obtain the discrete form of the integral term as follows:

[0069]

[0070] III. Iterative Extended Kalman Filter

[0071] I. Extended Kalman Filter

[0072] The classic state estimation model consists of a motion equation and an observation equation, as shown below:

[0073]

[0074] In the formula, f(·) represents the motion model, g(·) represents the observation model, and x k Let y be the state vector at time k. k v represents the observation at time k. k w represents the input at time k. k Let n be the motion noise at time k. k The observation noise at time k;

[0075] Considering that both the equations of motion and the observation equations are nonlinear in practice, the prior mean of the EKF estimate in the current state... At this point, a first-order Taylor expansion is performed to linearize the motion and observation model, resulting in:

[0076]

[0077] in, Let $\frac{ ...

[0078]

[0079] The observation model and its Jacobian matrix are written as follows:

[0080]

[0081]

[0082]

[0083] Derivation of the prediction x at the current time k and observation y k The statistical properties of the EKF are then substituted into the five classic formulas of the generalized Gaussian filter, ultimately yielding the five classic recursive update basic formulas of the EKF.

[0084] The predicted state equation is:

[0085]

[0086] The predicted covariance matrix is:

[0087]

[0088] The Kalman gain matrix is:

[0089]

[0090] The state equation for measurement updates is:

[0091]

[0092] The covariance equation for measurement updates is:

[0093]

[0094] Based on the two update equations above, x k-1 ,P k-1 Calculate x k ,P k This enables the transfer of state and variance;

[0095] II. Deriving IEKF from EKI

[0096] EKF uses the mean of the estimated state as the linearization operating point and discards higher-order error terms in the linearization approximation. When the system is highly nonlinear, the error will be large, and in some cases, this small error may lead to rapid divergence. Therefore, IEKF, based on EKF, changes the linearization operating point of the observation and gradually approximates the true state through iteration, thereby improving accuracy.

[0097] Modify the model observation equations in EKI for point x. op,k After linearization, the model observation equation in the above equation is rewritten as:

[0098] g(x k ,n k )≈y op,k +G k (x k -x op,k )+C k n k

[0099] At this point, the equations of motion remain unchanged, and both the observation model and its Jacobian matrix are in x. op,k If we linearize the expression, the corresponding equations in the above expression become:

[0100] y op,k =g(x op,k ,0)

[0101]

[0102]

[0103] Using the linearized model above, and based on the EKF derivation method, we can obtain five basic formulas for IEKF recursive updates; the only difference in formula form compared to EKF is the state equation for measurement updates:

[0104]

[0105] The five basic formulas for recursive updates in IEKF are extremely similar to those in EKF, the only difference being that IEKF uses the posterior mean to update the linearized working point; in EKF, the linearized working point is set as the prior mean. In its first iteration, IEKF made... Then, the Kalman gain matrix and posterior mean are repeatedly calculated, and in each iteration, the posterior mean of the previous iteration is used. Set as current working point x op,k When the termination condition is met, that is, the change between the posterior mean obtained in this iteration and the posterior mean of the previous iteration is small enough, it is considered that the Kalman gain matrix and the update equation have converged. Then, the posterior covariance equation is calculated again, and the final localization result is output.

[0106] Compared with existing technologies, this invention first installs a sensing system on the bridge crane to continuously acquire its current position information. The solid-state lidar within the sensing system collects environmental data in real time during operation and displays it on a terminal computer. The raw point cloud data collected by the solid-state lidar is converted into an octree map format in real time to build and update the environmental map. After building the entire working scene map, the octree map data is converted back into point cloud data, thus obtaining a priori environmental map of the bridge crane. Then, using the obtained solid-state lidar data, coarse matching based on point cloud descriptors is used to estimate the position and orientation of the bridge crane in the priori environmental map. Fine and coarse matching using the ICP algorithm are fused to determine the initial pose of the bridge crane. After obtaining the initial pose, the relative pose of the bridge crane in the map is estimated by combining the prior environmental map and the observation data from the solid-state lidar. The positioning result of the bridge crane is achieved using a tightly coupled fusion scheme of solid-state lidar and IMU data. Therefore, the relocation method of this invention can effectively ensure the accuracy of bridge crane positioning while maintaining good positioning efficiency. It solves the problem that existing solid-state lidar and IMU fusion methods rely on precise positioning information such as GPS and RTK for accurate positioning, thus providing reliable positioning data for subsequent tasks in a timely manner. Furthermore, the method of this invention does not result in a cumulative error that increases with operating time. Attached Figure Description

[0107] Figure 1 This is a diagram showing the sensor installation location in this invention;

[0108] Figure 2 This is a schematic diagram of the octree structure in this invention;

[0109] Figure 3 It is an octree format map of the prior environment map in this invention;

[0110] Figure 4 This is an overall flowchart of the relocation method in this invention. Detailed Implementation

[0111] The present invention will be further described below.

[0112] like Figure 1As shown, this invention first installs a sensing system on a bridge crane. The sensing system includes a solid-state lidar, an IMU sensor, and a computer. The computer includes a timestamp synchronization module, a data fusion module, and a display. The solid-state lidar is used to acquire point cloud images of the surrounding environment and transmit them to the computer. The IMU sensor is used to provide real-time spatial pose and acceleration information of the bridge crane and transmit it to the computer. The timestamp synchronization module in the computer is used to synchronize the clock of the solid-state lidar with the main clock of the computer. The data fusion module is used to fuse and analyze the data acquired by the solid-state lidar and the data fed back by the IMU sensor, and then transmit the final real-time positioning result to the display for display, such as... Figure 4 As shown, the specific relocation steps are as follows:

[0113] A. First, calibrate the solid-state lidar coordinate system and the bridge crane coordinate system separately. Then, transform the solid-state lidar coordinate system to the bridge crane coordinate system. The specific process is as follows: When the bridge crane is moving, the encoder on the bridge crane can obtain the real-time coordinates of the bridge crane. By setting three calibration objects, two sets of calibration object coordinates are obtained in the solid-state lidar coordinate system and the bridge crane coordinate system respectively. The transformation relationship between the solid-state lidar coordinate system and the bridge crane coordinate system is obtained using the two sets of calibration object coordinates, so as to realize the transformation of the solid-state lidar coordinate system to the bridge crane coordinate system.

[0114] B. The clock of the solid-state LiDAR is synchronized with the main clock of the computer through the timestamp synchronization module. The synchronization process uses the IEEE 1588v2.0 PTP Delay request-response mechanism (two steps). The Livox device acts as the slave end and performs PTP time synchronization with the computer clock device.

[0115] C. For example Figure 2 As shown, after acquiring real-time point cloud data using a solid-state LiDAR, an octree map is constructed. Then, the octree map is converted into a point cloud map and updated in real time. Figure 3 As shown, this allows us to obtain the prior environment map for relocation; the specific process of constructing the octree map is as follows:

[0116] In an octree structure, the root node represents a cube containing all the data in the entire dataset. The first-level octree evenly divides this cube into eight sub-cubes. Each sub-cube then becomes the parent node of the next level and is further divided into eight sub-cubes, until the amount of data in the cube is less than a preset number, or the specified octree level is reached, at which point the subtree is no longer divided.

[0117] (1) Determine the number of octree layers N by the leaf nodes and the actual range of the laser point cloud being processed. max It must satisfy the formula:

[0118] l0×2 Nmax ≥l max

[0119] Where l0 is the size of the leaf node of the octagonal tree, l max This represents the maximum side length of the point cloud spatial range, i.e., the root node size.

[0120] (2) Determine the voxel encoding of the leaf node where each point is located in the laser point cloud data. First, define the octree encoding method, the specific formula of which is:

[0121]

[0122] Where, m a It is an octal number representing the index of the node among its eight sibling nodes at the same level. Similarly, m a+1 m a The parent node is the index of its eight sibling nodes at the same level. The above formula fully describes the path from each leaf node to the root node in the octree. Therefore, the position of each leaf node in the spatial model is determined by its octree encoding. It should be noted that point cloud data is only stored in the leaf nodes, and intermediate nodes are only used as path channels during data retrieval.

[0123] Assuming the three-dimensional coordinates of the laser point are P(x,y,z), the minimum coordinate value of the vertex of the largest cube represented by the root node of the octree is expressed as (x...y...z). min ,y min ,z min If the coordinates are 3D, then the spatial index value Q(i,j,k) is obtained by calculation using the following formula:

[0124]

[0125] in, This indicates rounding down, converting the index value Q(i,j,k) to its binary representation:

[0126]

[0127] The octree encoding of the voxel containing the current laser point cloud data is then represented as:

[0128] m a =i a +j a 2 a +k a 2 2 , a∈{0,1,…,n-1}

[0129]

[0130] The number of leaf nodes at the bottom level of an octree structure is determined by the number of levels N. max This decision completes the construction of the octree map.

[0131] Assuming its maximum depth N max =16, then there are at most 2 16 ×2 16 ×2 16 Voxels. Assuming the octree resolution, i.e., the leaf node size l0 = 0.1m, then the range it can describe is x, y, z ∈ [-3276.8m, 3276.8m). The leaf nodes at the bottom level of the octree are not allocated memory space at initialization. When point cloud data for a certain region exists, the octree can divide it step by step downward from the root node according to the structure of the point cloud until all point cloud data is allocated to the bottom voxels.

[0132] D. Based on the prior environment map obtained in step C, a point cloud descriptor-based algorithm is used to coarsely match the current solid-state lidar observation data to create a ScanContext matrix. Then, the position and attitude of the bridge crane in the prior environment map are estimated based on the ScanContext matrix. The specific process of creating the ScanContext matrix is ​​as follows:

[0133] (1) Point cloud spatial partitioning

[0134] For a set P of laser point clouds, a secondary partitioning is performed along the radial and circumferential directions of the XOY plane. First, with the origin of the lidar coordinates as the center, the space is divided into N equidistant partitions along the direction of increasing radius. r Concentric rings; assuming the maximum sensing range of the lidar is L max Then the width of each ring is d. r :

[0135]

[0136] The second division involves cutting each ring into N equal-angled segments. s If the sector is a single sector, then the angle α of that single sector is... s for:

[0137]

[0138] Assumption Let represent the set of all laser points whose coordinate values ​​fall within the j-th sector region of the i-th annulus. Then, the set of laser point clouds in the segmented frame is represented as:

[0139]

[0140] (2) Generate Scan Context matrix

[0141] The point cloud segmented in the above manner is represented as an N r ×N s The matrix is ​​a series of points, where each row represents a ring in the actual space and each column corresponds to a sector within an angular range. Since the maximum height information of all 3D points within each sector is less affected by the environment, it effectively reflects the vertical shape characteristics of the point cloud structure within the region. Therefore, [the matrix is ​​used]. The maximum value of the Z-coordinate is taken as the characteristic value of this sector region, which is represented by the value φ of each element in the matrix as:

[0142]

[0143] Where z(p) represents Given the Z-coordinate of a point p within the surface, the Scan Context matrix I can be represented in the following form:

[0144]

[0145] If there are no points in the region, the feature value is set to 0;

[0146] (3) Similarity difference calculation

[0147] Definition I q For the Scan Context matrix of the point cloud data at the current time, I c Given the Scan Context matrix of point cloud data at a certain historical moment, then I q with I c The difference in similarity between them is defined as:

[0148]

[0149] In the formula, Indicate I q The j-th column in Indicate I c The j-th column in the array.

[0150] E. A fusion scheme based on fine matching using the ICP algorithm and coarse matching based on point cloud descriptors in step D (this process can be implemented using known methods) is used to determine the initial pose of the bridge crane; the specific implementation process of the ICP algorithm can be divided into the following steps:

[0151] (1) Based on the initial rotation matrix R0 and translation vector t0, transform the target point cloud into the space where the original point cloud is located;

[0152] (2) Determine the pair of matching points with the closest Euclidean distance between the original point cloud and the target point cloud;

[0153] (3) Based on the known matching point pairs, construct the overall target error function using the error of each group of points, and solve R and t using the SVD decomposition method or based on the idea of ​​nonlinear optimization.

[0154] (4) Determine whether the iteration termination condition is met, such as the target error function being less than a pre-set threshold, the change in the rotation matrix and translation vector in this iteration being less than the threshold compared to the previous iteration, or reaching the maximum number of iterations. If the iteration termination condition is met, the ICP algorithm ends and outputs R, t. If it does not converge, return to step B and repeat step BD to continuously optimize the registration error.

[0155] F. Based on the initial pose obtained in step E, and combined with the prior environment map and lidar observation data, estimate the relative pose of the bridge crane in the prior environment map. Then, use a tightly coupled fusion method between solid-state lidar and IMU to obtain the positioning result of the bridge crane. The specific process is as follows:

[0156] The tightly coupled IMU and lidar localization algorithm based on iterative extended Kalman filter (IEKF) can be divided into three modules: lidar odometry, IMU pre-integration, and iterative extended Kalman filter. The IEKF filter uses the pose obtained by the IMU pre-integration module as the prediction and the pose estimation result obtained by the feature-based lidar odometry as the observation. It fuses the prediction and observation data to update the system state and outputs the final local localization result.

[0157] I. Laser Odometry

[0158] ① First, select high-quality laser points based on four aspects: depth, refraction angle, intensity, and incident angle. The principles for eliminating bad points are: a. points with a large refraction angle; b. points with excessively high or low intensity; c. points with an excessively small incident angle; d. points that are obstructed. The specific reasons for each selection criterion are as follows:

[0159] a. Points with large refraction angles. Points with large refraction angles indicate that they are at the edge of the field of view. Points in this region usually exhibit severe distortion, resulting in a large deviation from the actual situation when calculating curvature, thus affecting the accuracy of feature extraction;

[0160] b. Points with excessively high or low intensity. If the calculated intensity is too low, it indicates that the point is far from the lidar origin or that the object has a low emissivity; if the intensity is too high, it will impact the lidar receiver and affect the measurement accuracy.

[0161] c. Points with excessively small incident angles; when the angle between the radar laser beam and the local plane containing the point is too small, the light spot will be elongated, which is not conducive to matching.

[0162] d. Occluded points: Points that are occluded will also be removed if two adjacent laser beams in the current frame are not actually on the same plane in the measurement.

[0163] ② Feature extraction is performed using the curvature magnitude. Curvature characterizes the local surface smoothness of the laser point. The specific formula for calculating curvature is as follows:

[0164]

[0165] in, and This represents the coordinates of the i-th and j-th scan points on the l-th laser beam in the k-th frame of data; This refers to the neighboring points near the target point with the same laser beam line number. Generally, five points on each side of the target point are used to calculate the curvature.

[0166] ③ Perform matching optimization by registering the feature points scanned in the k-th frame with the pre-constructed prior environment map. This yields the pose transformation relationship of the current frame's point cloud relative to the prior environment map, providing odometry output. Specifically, the feature point data of the current frame is projected onto the global map coordinate system, and n points corresponding to the target point are found within the global map. The distances from the points to the lines and from the points to the surfaces are calculated, and the residual function is constructed as follows:

[0167]

[0168]

[0169] In the formula, p w p is the projection point of the current frame feature point in the map coordinates. w The five nearest neighbors on the map. Before constructing the residuals and adding them to the pose optimization function, it is necessary to first determine whether these five points can form a line or a plane based on the characteristics of their covariance matrix. If the largest eigenvalue of the covariance matrix is ​​greater than three times the second largest eigenvalue, then the current neighboring points can be considered to form a straight line; if the second smallest eigenvalue of the covariance matrix is ​​greater than three times the smallest eigenvalue, then the current neighboring points can be considered to form a plane. Construct the optimization equation:

[0170]

[0171] II. IMU Pre-integration Algorithm

[0172] Based on the rigid body kinematics model in three-dimensional space, the differential equation of the IMU motion model is as follows:

[0173]

[0174] in, This represents the time derivative of the displacement of the IMU coordinate system relative to the global coordinate system at time t. This represents the derivative of the velocity with respect to time in the global coordinate system at time t. Let t represent the derivative of the quaternion of the IMU coordinate system with respect to the global coordinate system at time t with respect to time. This represents quaternion multiplication; the attitude description uses quaternion differential equations, which can also be written in the form of differential equations for rotation matrices:

[0175]

[0176] in Represents the rotational angular velocity, i.e., the angular velocity output of the gyroscope in actual use; [·]× indicates an antisymmetric matrix.

[0177] Based on the equations of motion, we can obtain the continuous integral results of the IMU from time i to time j:

[0178]

[0179] Define the integral term in the above formula as:

[0180]

[0181] Since the interval between two adjacent samples is short, it is assumed that the zero bias of the IMU remains constant within one sampling period, i.e., the bias is equal. However, as a state variable, changes in the bias will also cause changes in the integral term. Performing a Taylor expansion of the pre-integrated result at the bias, the integral term will then take the following form:

[0182]

[0183] The Jacobian matrix is ​​specifically represented as follows: The pre-integral formula is then rearranged as follows:

[0184]

[0185] Substituting the above formula and rearranging, we obtain the discrete form of the integral term as follows:

[0186]

[0187] III. Iterative Extended Kalman Filter

[0188] I. Extended Kalman Filter

[0189] The classic state estimation model consists of a motion equation and an observation equation, as shown below:

[0190]

[0191] In the formula, f(·) represents the motion model, g(·) represents the observation model, and x k Let y be the state vector at time k, which typically includes information such as the sensor's position, attitude, and velocity. k v represents the observation at time k. k w represents the input at time k. k Let n be the motion noise at time k. k The observation noise at time k; we usually assume w k and n k All of them follow a Gaussian distribution with zero mean, i.e., w k ~N(0,Q) k ), n k ~N(0,R k ), where Q k and R k Let be the covariance matrix of the noise.

[0192] Considering that both the equations of motion and the observation equations are nonlinear in practice, the prior mean of the EKF estimate in the current state... At this point, a first-order Taylor expansion is performed to linearize the motion and observation model, resulting in:

[0193]

[0194] in, Let $\frac{ ...

[0195]

[0196] The observation model and its Jacobian matrix are written as follows:

[0197]

[0198]

[0199]

[0200] Derivation of the prediction x at the current time k and observation y k The statistical properties of the EKF are then substituted into the five classic formulas of the generalized Gaussian filter, ultimately yielding the five classic recursive update basic formulas of the EKF.

[0201] The predicted state equation is:

[0202]

[0203] The predicted covariance matrix is:

[0204]

[0205] The Kalman gain matrix is:

[0206]

[0207] The state equation for measurement updates is:

[0208]

[0209] The covariance equation for measurement updates is:

[0210]

[0211] Based on the two update equations above, x k-1 ,P k-1 Calculate x k ,P k This enables the transfer of state and variance;

[0212] II. Deriving IEKF from EKI

[0213] EKF uses the mean of the estimated state as the linearization operating point and discards higher-order error terms in the linearization approximation. When the system is highly nonlinear, the error will be large, and in some cases, this small error may lead to rapid divergence. Therefore, IEKF, based on EKF, changes the linearization operating point of the observation and gradually approximates the true state through iteration, thereby improving accuracy.

[0214] Modify the model observation equations in EKI for point x. op,k After linearization, the model observation equation in the above equation is rewritten as:

[0215] g(x k ,n k )≈y op,k +G k (x k -x op,k )+C k n k

[0216] At this point, the equations of motion remain unchanged, and both the observation model and its Jacobian matrix are in x. op,k If we linearize the expression, the corresponding equations in the above expression become:

[0217] y op,k =g(x op,k ,0)

[0218]

[0219]

[0220] Using the linearized model above, and based on the EKF derivation method, we can obtain five basic formulas for IEKF recursive updates; the only difference in formula form compared to EKF is the state equation for measurement updates:

[0221]

[0222] The five basic formulas for recursive updates in IEKF are extremely similar to those in EKF, the only difference being that IEKF uses the posterior mean to update the linearized working point; in EKF, the linearized working point is set as the prior mean. In its first iteration, IEKF made... Then, the Kalman gain matrix and posterior mean are repeatedly calculated, and in each iteration, the posterior mean of the previous iteration is used. Set as current working point x op,k When the termination condition is met, that is, the change between the posterior mean obtained in this iteration and the posterior mean of the previous iteration is sufficiently small, such as... If the Kalman gain matrix and update equation have converged, then the posterior covariance equation is calculated again, and the final localization result is output.

[0223] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A repositioning method for an intelligent bridge crane, characterized in that, A sensing system is installed on the bridge crane. The sensing system includes a solid-state lidar, an IMU sensor, and a computer. The computer includes a timestamp synchronization module, a data fusion module, and a display. The solid-state lidar is used to acquire point cloud images of the surrounding environment and transmit them to the computer. The IMU sensor provides real-time spatial pose and acceleration information of the bridge crane and transmits it to the computer. The timestamp synchronization module in the computer synchronizes the clock of the solid-state lidar with the main clock of the computer. The data fusion module fuses and analyzes the data acquired by the solid-state lidar and the data fed back by the IMU sensor, and then transmits the final real-time positioning result to the display for display. The specific repositioning steps are as follows: A. First, calibrate the solid-state lidar coordinate system and the bridge crane coordinate system separately, and then transform the solid-state lidar coordinate system to the bridge crane coordinate system. B. Synchronize the clock of the solid-state lidar with the main clock of the computer through the timestamp synchronization module; C. After acquiring real-time point cloud data through solid-state LiDAR, an octree map is constructed. Then, the octree map is converted into a point cloud map and updated in real time to obtain the prior environment map in the relocalization process. D. Based on the prior environment map obtained in step C, use the point cloud descriptor algorithm to perform coarse matching on the current solid-state lidar observation data, create a Scan Context matrix, and then estimate the position and attitude of the bridge crane in the prior environment map based on the Scan Context matrix. E. A fusion scheme based on fine matching using the ICP algorithm and coarse matching based on point cloud descriptors in step D is used to determine the initial pose of the bridge crane. F. Based on the initial pose obtained in step E, and combined with the prior environment map and lidar observation data, estimate the relative pose of the bridge crane in the prior environment map. The positioning result of the bridge crane is obtained using a tightly coupled fusion method of solid-state lidar and IMU. Specifically, the tightly coupled IMU and lidar positioning algorithm based on iterative extended Kalman filter consists of three modules: laser odometry, IMU pre-integration module, and iterative extended Kalman filter. The iterative extended Kalman filter uses the pose obtained by the IMU pre-integration module as the prediction and the pose estimation result obtained by the feature-based laser odometry as the observation. The predicted and observed data are fused to update the system state, and the final local positioning result is output.

2. The repositioning method for an intelligent bridge crane according to claim 1, characterized in that, The specific process of step A is as follows: When the bridge crane is moving, the real-time coordinates of the bridge crane can be obtained through the encoder on the bridge crane. By setting three calibration objects, two sets of calibration object coordinates corresponding to the solid-state lidar coordinate system and the bridge crane coordinate system are obtained respectively. The transformation relationship between the solid-state lidar coordinate system and the bridge crane coordinate system is obtained by using the two sets of calibration object coordinates, so as to realize the transformation from the solid-state lidar coordinate system to the bridge crane coordinate system.

3. The repositioning method for an intelligent bridge crane according to claim 1, characterized in that, In step B, the time synchronization process uses the IEEE 1588v2.0 PTP Delay request-response mechanism, with the Livox device acting as a slave to synchronize time with the computer clock device via PTP.

4. The repositioning method for an intelligent bridge crane according to claim 1, characterized in that, The specific process of constructing the octree map in step C is as follows: (1) Determine the number of octree layers based on leaf nodes and the actual range of laser point cloud processed. It must satisfy the formula: in The dimensions of the leaf nodes of an octagonal tree. This represents the maximum side length of the point cloud spatial range, i.e., the root node size. (2) Determine the voxel encoding of the leaf node where each point is located in the laser point cloud data. First, define the octree encoding method, and the specific formula is as follows: in, It is an octal number representing the index of the node among its eight sibling nodes at the same level. Similarly, express The index of the parent node in its eight sibling nodes at the same level is used to determine the position of each leaf node in the spatial model based on its octree encoding. Assume the three-dimensional coordinates of the laser point are The root node of the octree represents the minimum coordinate value of the largest cube vertex. The spatial index value is then obtained by calculating the three-dimensional coordinates. The calculation formula is as follows: in, This indicates rounding down to the nearest integer, using the index value. Convert to binary representation: The octree encoding of the voxel containing the current laser point cloud data is then represented as: The number of leaf nodes at the bottom level of an octree structure is determined by the number of levels. This decision completes the construction of the octree map.

5. The repositioning method for an intelligent bridge crane according to claim 1, characterized in that, The specific process of creating the Scan Context matrix in step D is as follows: (1) Point cloud spatial division For a set P of laser point clouds, a secondary partitioning is performed along the radial and circumferential directions of the XOY plane. First, with the origin of the lidar coordinates as the center, the space is equidistantly divided into a number of partitions along the direction of increasing radius. The concentric rings; assuming the maximum sensing range of the lidar is Then the width of each ring is : The second division involves cutting each ring at equal angles into a number of... The angle of a single sector region. for: Assumption Let represent the set of all laser points whose coordinate values ​​fall within the j-th sector region of the i-th annulus. Then, the set of laser point clouds in the segmented frame is represented as: (2) Generate Scan Context matrix The point cloud segmented in the above manner is represented as a single point cloud. The matrix is ​​a series of points, where each row represents a ring in the actual space and each column corresponds to a sector within an angular range. Since the maximum height information of all 3D points within each sector is less affected by the environment, it effectively reflects the vertical shape characteristics of the point cloud structure within the region. Therefore, [the matrix is ​​used]. The maximum value of the Z-coordinate is taken as the characteristic value of the sector region, which is the value of each element in the matrix. Represented as: in express Given the Z-coordinate of a point p within the interior, the Scan Context matrix I can be represented in the following form: If there are no points in the region, the feature value is set to 0; (3) Similarity difference calculation definition The Scan Context matrix represents the point cloud data at the current moment. Given the ScanContext matrix of point cloud data at a certain historical moment, then... and The difference in similarity between them is defined as: In the formula, express The j-th column in express The j-th column in the array.

6. The repositioning method for an intelligent bridge crane according to claim 1, characterized in that, The specific process of step F is as follows: I. Laser Odometry ① First, select high-quality laser points based on four aspects: depth, refraction angle, intensity, and incident angle. The principles for eliminating badpoints are: a. points with a large refraction angle; b. points with excessively high or low intensity; c. points with excessively small incident angle. d. The obscured point; ② Feature extraction is performed using the curvature magnitude. Curvature characterizes the local surface smoothness of the laser point. The specific formula for calculating curvature is as follows: in, and This represents the coordinates of the i-th and j-th scan points on the l-th laser beam in the k-th frame of data; This indicates the nearest point to the target point and its neighboring points with the same laser beam number; ③ Perform matching optimization, register the feature points scanned in the kth frame with the already constructed prior environment map, and obtain the pose transformation relationship of the current frame point cloud relative to the prior environment map, providing odometry output; II. IMU Pre-integration Algorithm Based on the rigid body kinematics model in three-dimensional space, the differential equation of the IMU motion model is as follows: in, This represents the time derivative of the displacement of the IMU coordinate system relative to the global coordinate system at time t. This represents the derivative of the velocity with respect to time in the global coordinate system at time t. Let t represent the derivative of the quaternion of the IMU coordinate system with respect to the global coordinate system at time t with respect to time. This represents quaternion multiplication; the attitude description uses quaternion differential equations, which can also be written in the form of differential equations for rotation matrices: in This represents the rotational angular velocity, which is the angular velocity output of the gyroscope during actual use. Indicates an antisymmetric matrix; Based on the equations of motion, we can obtain the continuous integral results of the IMU from time i to time j: Define the integral term in the above formula as: Since the interval between two adjacent samples is short, it is assumed that the zero bias of the IMU remains constant within one sampling period, i.e., the bias is equal. However, as a state variable, changes in the bias will also cause changes in the integral term. Performing a Taylor expansion of the pre-integrated result at the bias, the integral term will then take the following form: The Jacobian matrix is ​​specifically represented as follows: , , , , The pre-integral formula is then simplified to: Substituting the above formula and rearranging, we obtain the discrete form of the integral term as follows: III. Iterative Extended Kalman Filter I. Extended Kalman Filter The classic state estimation model consists of a motion equation and an observation equation, as shown below: In the formula For motion model, For observation models, Let k be the state vector at time k. Represents the observation at time k. This represents the input at time k. The motion noise at time k, The observation noise at time k; Considering that both the equations of motion and the observation equations are nonlinear in practice, the prior mean of the EKF estimate in the current state... At this point, a first-order Taylor expansion is performed to linearize the motion and observation model, resulting in: in, Let $\frac{ ... The observation model and its Jacobian matrix are written as follows: Derivation of the current time prediction and observation The statistical properties of the EKF are then substituted into the five classic formulas of the generalized Gaussian filter, ultimately yielding the five classic recursive update basic formulas of the EKF. The predicted state equation is: The predicted covariance matrix is: The Kalman gain matrix is: The state equation for measurement updates is: The covariance equation for measurement updates is: Based on the two update equations above, calculate This enables the transfer of state and variance; II. Deriving IEKF from EKI EKF uses the mean of the estimated state as the linearization working point and iteratively approximates the true state to improve accuracy. Modify the model observation equations in EKI for the points After linearization, the model observation equation in the above equation is rewritten as: At this point, the equations of motion remain unchanged, and the observation model and its Jacobian matrix are both in... If we linearize the expression, the corresponding equations in the above expression become: Using the linearized model above, and based on the EKF derivation method, we can obtain five basic formulas for IEKF recursive updates; the only difference in formula form compared to EKF is the state equation for measurement updates: The five basic formulas for recursive updates in IEKF are extremely similar to those in EKF, the only difference being that IEKF uses the posterior mean to update the linearized working point; in EKF, the linearized working point is set as the prior mean. In its first iteration, IEKF made... Then, the Kalman gain matrix and posterior mean are repeatedly calculated, and in each iteration, the posterior mean of the previous iteration is used. Set as current working point When the termination condition is met, that is, the change between the posterior mean obtained in this iteration and the posterior mean of the previous iteration is small enough, it is considered that the Kalman gain matrix and the update equation have converged. Then, the posterior covariance equation is calculated again, and the final localization result is output.

Images