Joint calibration method, device and system for laser radar and forklift inner and outer parameters

Abstract

This invention discloses a method, device, and system for joint calibration of forklift intrinsic and extrinsic parameters based on lidar and forklifts. The method includes controlling a forklift equipped with lidar to move at a constant speed along an S-shaped or 8-shaped path, acquiring steering wheel data and lidar-recorded laser data during forklift movement; constructing a forklift coordinate system and a lidar coordinate system; establishing an overdetermined linear equation system about the forklift steering wheel and fork arm wheel assembly based on the transformations of the steering wheel data and lidar coordinate system during forklift movement, and solving the overdetermined linear equation system using the least squares method to obtain the forklift intrinsic parameter ratio; determining the forklift transformation relationship and lidar transformation relationship based on the steering wheel data and laser data during forklift movement and constructing an error function; optimizing the error function based on the forklift intrinsic parameter ratio to obtain the forklift intrinsic parameter value and lidar extrinsic parameter value; and calibrating the forklift intrinsic parameter value and lidar extrinsic parameter value based on the forklift intrinsic parameter value and lidar extrinsic parameter value. This invention completes the calibration of the intrinsic and extrinsic parameters of an unmanned forklift without the aid of calibration tools.

Description

Technical Field

[0001] This invention relates to the field of lidar calibration technology, and in particular to a method, apparatus and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters. Background Technology

[0002] More and more logistics and warehousing centers are using automated forklifts (AGTs) to replace manual labor for handling goods, typically pallets or cages. Before an AGT can perform a handling task, it needs to create a map using data from a 2D LiDAR mounted on its top and synchronous positioning and mapping (SMR) technology. During normal handling operations, the AGT automatically moves to the goods to be handled, picks them up, and moves them to a designated location in the warehouse. The entire process requires close coordination between the 2D LiDAR, the main steering wheel, and the fork arm end caps. The AGT relies on LiDAR data to locate itself on the map, the main steering wheel on the chassis to control its movement and steering, and precise control of the fork arm end cap position to ensure accurate reach to the bottom holes of the pallet or cage for picking. The 2D LiDAR, main steering wheel, and fork arm end caps are mechanically connected to the AGT. Machining and assembly errors in the machining and assembly processes cause deviations in the positional relationship between the LiDAR, main steering wheel, and fork arm end caps from the theoretical design. Summary of the Invention

[0003] This invention provides a method, apparatus, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters, which can achieve automatic joint calibration of the intrinsic and extrinsic parameters of LiDAR, main steering wheel, and fork arm end without the aid of calibration templates or other calibration tools.

[0004] To address the aforementioned technical problems, this invention provides a method, apparatus, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters, comprising the following steps:

[0005] The forklift is controlled to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and real-time laser data recorded by the lidar during the forklift's movement.

[0006] Construct the forklift coordinate system and the lidar coordinate system;

[0007] Based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement, an overdetermined linear equation system about the forklift steering wheel and fork arm wheel assembly is established, and the internal reference value of the forklift is obtained by solving the overdetermined linear equation system using the least squares method.

[0008] Based on the steering wheel data and laser data during forklift movement, the forklift transformation relationship and the lidar transformation relationship are determined. Based on the forklift transformation relationship and the lidar transformation relationship, an error function is constructed. Then, based on the forklift intrinsic parameter ratio, the error function is optimized to obtain the forklift intrinsic parameter value and the lidar extrinsic parameter value.

[0009] The forklift's intrinsic parameters and the lidar's extrinsic parameters are calibrated based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

[0010] This invention proposes a method, device, and system for joint calibration of forklift intrinsic and extrinsic parameters based on LiDAR and forklifts. By having a forklift move at a constant speed along an S-shaped or 8-shaped path, and without the aid of calibration templates or other calibration tools, the system constructs a forklift coordinate system, a LiDAR coordinate system, and forklift and LiDAR transformation relationships using steering wheel data and LiDAR data during forklift movement. Furthermore, it establishes an overdetermined linear equation set between the forklift steering wheel and the fork arm wheel assembly using steering wheel data and the LiDAR coordinate system. The system then calculates the forklift intrinsic parameter ratio using the least squares method, obtaining an accurate intrinsic parameter ratio in a short time. Finally, it constructs an error function using the forklift and LiDAR transformation relationships, and combines this with the forklift intrinsic parameter ratio to calculate the forklift intrinsic and extrinsic parameters, thereby calibrating the forklift's intrinsic and extrinsic parameters and completing the calibration of the 2D LiDAR and the forklift's extrinsic parameters on the unmanned forklift.

[0011] As a preferred example, the establishment of an overdetermined linear equation system regarding the forklift steering wheel and fork arm wheel assembly based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement is specifically as follows:

[0012] The angular velocity and yaw angle of the steering wheel around the center of the steering wheel are obtained based on the steering wheel data when the forklift moves, and the rotation angle of the lidar coordinate system is obtained based on the transformation of the lidar coordinate system.

[0013] A first matrix is ​​constructed based on the angular velocity of the steering wheel around the center of the steering wheel, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift is moving, and an overdetermined linear equation system related to the first matrix is ​​established.

[0014] This preferred example establishes an overdetermined linear equation system based on the angular velocity of the steering wheel around its center, the steering wheel yaw angle, and the rotation angle of the lidar coordinate system during forklift movement. This system can be used to solve optimization problems with extremely high complexity, and even nonlinear problems, possessing more variables and constraints than ordinary linear equations. It can solve complex forklift trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0015] As a preferred example, the overdetermined linear equation system is in the form AX = B;

[0016] Specifically, the overdetermined linear equation system is as follows:

[0017]

[0018] Where, ω s θ is the angular velocity of the steering wheel around its center when the forklift moves, t is time, and θ is the angular velocity of the steering wheel. s For the yaw angle of the steering wheel, This represents the rotation angle between adjacent frames in the lidar coordinate system.

[0019] This preferred example establishes an overdetermined linear equation system by using the angular velocity of the steering wheel around the center of the steering wheel during forklift movement, the rotation angle of the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle of adjacent frames in the lidar coordinate system. This system can solve complex forklift movement trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0020] As a preferred example, the method of obtaining the forklift internal reference value by solving the overdetermined linear equations using the least squares method specifically involves:

[0021] The overdetermined linear equations are transformed using the least squares method to obtain a modified formula. Solving the modified formula yields the forklift internal reference ratio value:

[0022]

[0023] Where, ΔT k For a given time interval [t] k , t k+1 ], For time interval [t] k , t k+1 When the forklift moves within the [location], the angular velocity of the steering wheel around its center is [missing value]. For time interval [t] k , t k+1 The yaw angle of the steering wheel within [the area]. For time interval [t] k , t k+1 The rotation angle of adjacent frames in the lidar coordinate system within the [].

[0024] This preferred example uses the least squares method to transform and solve the existing equations of the upward-pointing system, which can minimize the error, effectively solve complex nonlinear equations, satisfy all constraints, and effectively obtain the optimal solution.

[0025] As a preferred example, the transformed formula is in the form of x=(A T A) -1 A T B.

[0026] This preferred example uses the least squares method to transform and solve the existing equations of the upward-pointing system, which can minimize the error, effectively solve complex nonlinear equations, satisfy all constraints, and effectively obtain the optimal solution.

[0027] As a preferred example, the step of constructing an error function based on the forklift transformation relationship and the lidar transformation relationship is as follows:

[0028] An error function is constructed by analyzing the transformation relationships of forklifts and lidar.

[0029] The error function is:

[0030]

[0031] in, Let s be the error value. k For the transformation relationship of lidar, r k Forklift transformation relationship, This is the extrinsic parameter of the lidar coordinate system in the forklift coordinate system.

[0032] This preferred example transforms the intrinsic and extrinsic parameter calibration problem into an optimization problem by constructing an error function, and then seeks the maximum value of this error function. Since the forklift transformation relationship is theoretically derived based on the forklift body model and the integral of the main steering wheel angular velocity, it is a function containing forklift body parameters, i.e., the intrinsic parameters of the forklift body. In practical applications, 2D LiDAR data also contains a certain amount of noise, making it impossible to obtain ideally accurate values ​​for estimating the LiDAR transformation relationship, thus leading to error accumulation. The smaller the accumulated error, the closer the corresponding sensor extrinsic parameters are to the true value.

[0033] As a preferred example, the subsequent optimization of the error function based on the forklift intrinsic parameter ratio to obtain the forklift intrinsic and extrinsic parameter values ​​specifically involves:

[0034] The error function is optimized based on the forklift internal reference value, and transformed into a corresponding quadratic problem. The Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is constructed, and the corresponding homogeneous linear equation system is extracted from it.

[0035] Solve the corresponding homogeneous linear equation system to obtain the first solution vector and the second solution vector;

[0036] By combining the corresponding homogeneous linear equation system with the quadratic problem, and substituting the first solution vector and the second solution vector into the error function, the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value is taken.

[0037] The intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR are calculated based on the position matrix value of the LiDAR coordinate system with the smallest function value in the forklift coordinate system.

[0038] This preferred example, by constructing a homogeneous linear equation, can be used to solve for the unknown variable in the sparse matrix of forklift motion data that is closest to a specific target value. It can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0039] As a preferred example, the optimization of the error function based on the forklift internal reference ratio is transformed into a corresponding quadratic problem. The Lagrangian function of the position matrix of the LiDAR coordinate system in the forklift coordinate system is constructed, and the corresponding homogeneous linear equations are extracted from it. Specifically:

[0040] The error function is optimized by combining the forklift internal reference ratio as follows:

[0041]

[0042] Transform into the corresponding quadratic form problem The corresponding quadratic form problem is:

[0043]

[0044]

[0045]

[0046] The Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is constructed using Lagrange multiplication, wherein the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is:

[0047]

[0048] The Lagrange function is transformed, and according to... The physical meaning is extracted to obtain the corresponding homogeneous linear equation system, which is:

[0049]

[0050] in, Q k Let k be the construction matrix for the quadratic form problem at time k. Let C be the vector combining the distance from the forklift steering wheel to the forklift arm assembly and the pose of the LiDAR coordinate system in the forklift coordinate system, where C is a constant independent of the optimization variables. xLet l be the position of the lidar coordinate system under the X-axis of the vehicle coordinate system. y Let l be the position of the lidar coordinate system under the Y-axis of the vehicle coordinate system. θ Let w be the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise; w is a matrix.

[0051] This preferred example, by constructing a homogeneous linear equation, can be used to solve for the unknown variable in the sparse matrix of forklift motion data that is closest to a specific target value. It can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0052] As a preferred example, the transformation of the Lagrange function specifically includes:

[0053] By differentiating and transforming the Lagrange function, we obtain the following formula:

[0054]

[0055] in, Let λ be the difference, and λ be the root of the required solution.

[0056] This preferred example uses the derivative of the Lagrange function to obtain the corresponding homogeneous linear equation system.

[0057] As a preferred example, the step of combining the corresponding homogeneous linear equation system with the quadratic problem, substituting the first and second solution vectors into the error function, and taking the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value, is specifically as follows:

[0058] Combining the corresponding homogeneous linear equation system with the quadratic form problem, we obtain the corresponding formula:

[0059]

[0060] Substitute the first and second solution vectors into the error function, and take the position matrix value of the LiDAR coordinate system in the forklift coordinate system that has the smallest function value. The smallest position matrix value of the LiDAR coordinate system in the forklift coordinate system is:

[0061]

[0062] Based on the error function, we can obtain:

[0063]

[0064] in, Let L represent the relevant positions of the LiDAR coordinate system in the forklift coordinate system, and L be the distance from the steering wheel to the forklift wheel assembly. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0065] This preferred example obtains the position matrix value of the LiDAR coordinate system in the forklift coordinate system by solving the error function, so as to further calculate the forklift's intrinsic and extrinsic parameters.

[0066] As a preferred example, the calculation of the forklift intrinsic parameters and the lidar extrinsic parameters based on the position matrix value of the lidar coordinate system with the smallest function value in the forklift coordinate system is specifically as follows:

[0067] The intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR are calculated based on the position matrix value of the LiDAR coordinate system in the forklift coordinate system, which has the smallest function value. The intrinsic and extrinsic parameters are as follows:

[0068]

[0069]

[0070]

[0071]

[0072]

[0073] The intrinsic parameter value is the steering wheel radius r. s The distance L from the steering wheel to the forklift wheel assembly, and the extrinsic parameter value is the pose of the 2D LiDAR in the forklift body coordinate system. Among them, l x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system. θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0074] This preferred example calculates the forklift's intrinsic and extrinsic parameters using the position matrix value of the smallest LiDAR coordinate system in the forklift coordinate system.

[0075] This invention also proposes a joint calibration device based on lidar and forklift intrinsic and extrinsic parameters, including an acquisition module, a construction module, a solution module, a calculation module, and a calibration module;

[0076] The acquisition module is used to control the forklift to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and laser data recorded in real time by the lidar when the forklift is moving.

[0077] The construction module is used to construct the forklift coordinate system and the lidar coordinate system;

[0078] The solution module establishes an overdetermined linear equation set about the forklift steering wheel and fork arm wheel assembly based on the steering wheel data and the transformation of the lidar coordinate system when the forklift moves, and obtains the forklift internal reference value by solving the overdetermined linear equation set using the least squares method.

[0079] The calculation module is used to determine the forklift transformation relationship and the lidar transformation relationship based on the steering wheel data and the laser data when the forklift is moving, and to construct an error function based on the forklift transformation relationship and the lidar transformation relationship. Then, the error function is optimized based on the forklift intrinsic parameter ratio to obtain the forklift intrinsic parameter value and the lidar extrinsic parameter value.

[0080] The calibration module is used to calibrate the forklift's intrinsic parameters and the lidar's extrinsic parameters based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

[0081] This invention acquires data from the steering wheel and laser sensors by using a module to move the forklift at a constant speed along an S-shaped or 8-shaped path, without the aid of calibration templates or other calibration tools. It then constructs a forklift coordinate system, a laser radar coordinate system, and transformation relationships between the forklift and laser radar. Using the steering wheel data and laser radar coordinate system, it further establishes an overdetermined linear equation set between the forklift steering wheel and the fork arm wheel assembly. The forklift's internal parameter ratio is calculated using the least squares method, achieving high accuracy in a short time. The calculation module further constructs an error function by calculating the transformation relationships between the forklift and laser radar, and combines this with the internal parameter ratio to calculate the forklift's internal and external parameters for calibration. Finally, the calibration module completes the calibration of the 2D laser radar and the forklift's external parameters on the unmanned forklift.

[0082] As a preferred example, the solution module includes a first unit and a second unit;

[0083] The first unit is used to obtain the angular velocity of the steering wheel around the center of the steering wheel and the yaw angle of the steering wheel when the forklift moves, based on the steering wheel data when the forklift moves, and to obtain the rotation angle of the lidar coordinate system based on the transformation of the lidar coordinate system.

[0084] The second unit is used to construct a first matrix based on the angular velocity of the steering wheel around the center of the steering wheel when the forklift moves, the rotation angle of the forklift coordinate system, the yaw angle of the steering wheel and the rotation angle of the lidar coordinate system, and to establish an overdetermined linear equation system related to the first matrix.

[0085] This preferred example establishes an overdetermined linear equation system based on the angular velocity of the steering wheel around its center, the rotation angle in the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle in the lidar coordinate system during forklift movement. This system can be used to solve optimization problems with extremely high complexity, and even nonlinear problems, possessing more variables and constraints than general linear equations. It can solve complex forklift trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0086] As a preferred example, the overdetermined linear equation system is in the form AX = B;

[0087] Specifically, the overdetermined linear equation system is as follows:

[0088]

[0089] Where, ω s θ is the angular velocity of the steering wheel around its center when the forklift moves, t is time, and θ is the angular velocity of the steering wheel. s For the yaw angle of the steering wheel, This represents the rotation angle between adjacent frames in the lidar coordinate system.

[0090] This preferred example establishes an overdetermined linear equation system by considering the angular velocity of the steering wheel around the steering wheel center, the rotation angle of the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system during forklift movement. This system can solve complex forklift movement trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0091] As a preferred example, the solution module further includes a third unit and a fourth unit;

[0092] The third unit is used to transform the overdetermined linear equations using the least squares method to obtain a deformed formula, and then solves the deformed formula to obtain the forklift internal reference value:

[0093]

[0094] Where, ΔT k For a given time interval [t] k , t k+1 ], For time interval [t] k , t k+1 When the forklift moves within the [location], the angular velocity of the steering wheel around its center is [missing value]. For time interval [t] k , t k+1 The yaw angle of the steering wheel within [the area]. For time interval [t] k , t k+1 The rotation angle of adjacent frames in the lidar coordinate system within the [].

[0095] This preferred example uses the least squares method to transform and solve the existing equations of the upward-pointing system, which can minimize the error, effectively solve complex nonlinear equations, satisfy all constraints, and effectively obtain the optimal solution.

[0096] As a preferred example, the transformed formula is in the form X = (A T A) -1 A T B.

[0097] This preferred example uses the least squares method to transform and solve the existing equations of the upward-pointing system, which can minimize the error, effectively solve complex nonlinear equations, satisfy all constraints, and effectively obtain the optimal solution.

[0098] As a preferred example, the computing module includes a fifth unit;

[0099] The fifth unit is used to construct an error function based on the forklift transformation relationship and the lidar transformation relationship;

[0100] The error function is:

[0101]

[0102] in, Let s be the error value. k For the transformation relationship of lidar, r k Forklift transformation relationship, This is the extrinsic parameter of the lidar coordinate system in the forklift coordinate system.

[0103] This preferred example transforms the intrinsic and extrinsic parameter calibration problem into an optimization problem by constructing an error function, and then seeks the maximum value of this error function. Since the forklift transformation relationship is theoretically derived based on the forklift body model and the integral of the main steering wheel angular velocity, it is a function containing forklift body parameters, i.e., the intrinsic parameters of the forklift body. In practical applications, 2D LiDAR data also contains a certain amount of noise, making it impossible to obtain ideally accurate values ​​for estimating the LiDAR transformation relationship, thus leading to error accumulation. The smaller the accumulated error, the closer the corresponding sensor extrinsic parameters are to the true value.

[0104] As a preferred example, the computing module further includes a sixth unit, a seventh unit, an eighth unit, and a ninth unit;

[0105] The sixth unit is used to optimize the error function based on the forklift internal reference value, convert it into a corresponding quadratic problem, construct the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system, and extract the corresponding homogeneous linear equation system from it.

[0106] The seventh unit is used to solve the corresponding homogeneous linear equation system to obtain the first solution vector and the second solution vector.

[0107] The eighth unit is used to combine the corresponding homogeneous linear equation system with the quadratic problem, and substitute the first solution vector and the second solution vector into the error function respectively, and take the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value.

[0108] The ninth unit is used to calculate the intrinsic parameters of the forklift and the extrinsic parameters of the lidar based on the position matrix value of the lidar coordinate system with the smallest function value in the forklift coordinate system.

[0109] This preferred example, by constructing a homogeneous linear equation, can be used to solve for the unknown variable in the sparse matrix of forklift motion data that is closest to a specific target value. It can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0110] As a preferred example, the sixth unit includes a first subunit, a second subunit, a third subunit, and a fourth subunit;

[0111] The first subunit is used to optimize the error function by combining the forklift internal reference ratio:

[0112]

[0113] The second sub-unit is used to transform the problem into the corresponding quadratic form. The corresponding quadratic form problem is:

[0114]

[0115]

[0116]

[0117] The third subunit is used to construct the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system using Lagrange multiplication, wherein the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is:

[0118]

[0119] The fourth sub-unit is used to transform the Lagrange function, and according to... The physical meaning is extracted to obtain the corresponding homogeneous linear equation system, which is:

[0120]

[0121] in, Q k Let k be the construction matrix for the quadratic form problem at time k. Let C be the vector combining the distance from the forklift steering wheel to the forklift arm assembly and the pose of the LiDAR coordinate system in the forklift coordinate system, where C is a constant independent of the optimization variables. x Let l be the position of the lidar coordinate system under the X-axis of the vehicle coordinate system. y Let l be the position of the lidar coordinate system under the Y-axis of the vehicle coordinate system. θ Let w be the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise; w is a matrix.

[0122] This preferred example, by constructing a homogeneous linear equation, can be used to solve for the unknown variable in the sparse matrix of forklift motion data that is closest to a specific target value. It can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0123] As a preferred example, the fourth sub-unit includes the first sub-module;

[0124] The first submodule is used to differentiate and transform the Lagrange function to obtain the formula:

[0125]

[0126] in, Let λ be the difference, and λ be the root of the required solution.

[0127] This preferred example uses the derivative of the Lagrange function to obtain the corresponding homogeneous linear equation system.

[0128] As a preferred example, the eighth unit includes a fifth subunit, a sixth subunit, and a seventh subunit;

[0129] The fifth subunit is used to combine the corresponding homogeneous linear equation system with the quadratic problem to obtain the corresponding formula:

[0130]

[0131] The sixth subunit is used to substitute the first and second solution vectors into the error function, and take the position matrix value of the LiDAR coordinate system in the forklift coordinate system with the smallest function value. The smallest position matrix value of the LiDAR coordinate system in the forklift coordinate system is:

[0132]

[0133] The seventh sub-unit is used to obtain the following from the error function:

[0134]

[0135] in, Let L represent the relevant positions of the LiDAR coordinate system in the forklift coordinate system, and L be the distance from the steering wheel to the forklift wheel assembly. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system. θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0136] This preferred example obtains the position matrix value of the LiDAR coordinate system in the forklift coordinate system by solving the error function, so as to further calculate the forklift's intrinsic and extrinsic parameters.

[0137] As a preferred example, the ninth unit includes an eighth subunit;

[0138] The eighth subunit is used to calculate the intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR based on the position matrix value of the LiDAR coordinate system in the forklift coordinate system with the smallest function value. The intrinsic and extrinsic parameters are as follows:

[0139]

[0140]

[0141]

[0142]

[0143]

[0144] The intrinsic parameter value is the steering wheel radius r. s The distance L from the steering wheel to the forklift wheel assembly is given by the extrinsic parameter l, which represents the pose of the 2D LiDAR in the forklift's body coordinate system. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system. θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0145] This preferred example calculates the forklift's intrinsic and extrinsic parameters using the position matrix value of the smallest LiDAR coordinate system in the forklift coordinate system.

[0146] This invention also proposes a joint calibration system based on lidar and forklift intrinsic and extrinsic parameters, including a forklift, lidar, and computing devices;

[0147] The forklift is used to carry the lidar and move at a constant speed along an S-shaped or 8-shaped route;

[0148] The lidar is used to emit lasers and record laser data in real time;

[0149] The computing device is used to execute the above-mentioned joint calibration method based on lidar and forklift intrinsic and extrinsic parameters.

[0150] In this preferred example, the forklift moves at a constant speed along an S-shaped or 8-shaped route; the lidar emits lasers and records laser data in real time; without the aid of calibration templates or other calibration tools, the computing device constructs a forklift coordinate system, a lidar coordinate system, a forklift transformation relationship, and a lidar transformation relationship using the steering wheel data and laser data during forklift movement. The forklift coordinate system and the lidar coordinate system are further used to establish an overdetermined linear equation set for the forklift steering wheel and fork arm wheel assembly. The forklift's internal parameter ratio is calculated using the least squares method, obtaining an accurate internal parameter ratio in a short time. An error function is further constructed using the forklift transformation relationship and the lidar transformation relationship, and the forklift's internal and external parameter values ​​are calculated based on the internal parameter ratio to calibrate the forklift's internal and external parameters, thus completing the calibration of the 2D lidar and the forklift's external parameters on the unmanned forklift.

[0151] This invention proposes a method, device, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters. By having the forklift move at a constant speed along an S-shaped or 8-shaped path, data from the steering wheel and LiDAR are acquired during the forklift's movement without the aid of calibration templates or other calibration tools. A forklift coordinate system, a LiDAR coordinate system, forklift transformation relationships, and LiDAR transformation relationships are constructed. Furthermore, overdetermined linear equations for the forklift steering wheel and fork arm wheel assembly are established for the forklift and LiDAR coordinate systems. The least squares method is used to calculate the forklift intrinsic parameter ratio, obtaining an accurate intrinsic parameter ratio in a short time. The forklift transformation relationships and LiDAR transformation relationships are calculated to further construct an error function. Combined with the forklift intrinsic parameter ratio, the intrinsic and extrinsic parameters of the forklift are calculated for calibration, ultimately completing the calibration of the 2D LiDAR and the forklift's extrinsic parameters on the unmanned forklift. Attached Figure Description

[0152] Figure 1 This is a flowchart of a method, apparatus, and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to a certain embodiment of the present invention;

[0153] Figure 2 This is a stack diagram of a method, apparatus and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to a certain embodiment of the present invention;

[0154] Figure 3This is a diagram of a material cage based on a method, apparatus, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters according to a certain embodiment of the present invention.

[0155] Figure 4 This is a diagram showing the installation position of a 2D lidar on a single-steering-wheel forklift, based on a method, apparatus, and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to an embodiment of the present invention.

[0156] Figure 5 This is an illustration of the components of a single-steering wheel forklift in a top-view state, based on a method, device and system for joint calibration of lidar and forklift internal and external parameters according to a certain embodiment of the present invention.

[0157] Figure 6 This is a calibration concept diagram of a method, device, and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to a certain embodiment of the present invention;

[0158] Figure 7 This is a motion model diagram of a single-steering wheel robot based on a joint calibration method, device and system for laser radar and forklift intrinsic and extrinsic parameters according to an embodiment of the present invention;

[0159] Figure 8 This is a device module diagram of a method, apparatus and system for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to a certain embodiment of the present invention. Detailed Implementation

[0160] The technical solutions of this invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0161] Explanation of the technical terms used in the text:

[0162] Simultaneous Localization and Mapping (SLAM) is a computational problem that involves building or updating a map of an unknown environment while simultaneously tracking the agent's position within it. Approximate solution methods include particle filters, extended Kalman filters, covariance intersection, and graph SLAM. These algorithms are based on concepts from computational geometry and computer vision and are used for robot navigation, robot mapping, and odometry in virtual reality or augmented reality. They are tailored to available resources, prioritizing operational compliance rather than perfection.

[0163] The method, apparatus, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters provided in this invention are applicable to [various applications].

[0164] Please see Figure 1 In one embodiment of the present invention, a method is provided. Figure 1 The flowchart shown illustrates a method, apparatus, and system for joint calibration of intrinsic and extrinsic parameters of a LiDAR and forklift. This method includes steps S1 to S5. The specific steps are as follows:

[0165] S1. Control the forklift to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and real-time laser data recorded by the lidar during the forklift's movement.

[0166] S2. Construct the forklift coordinate system and the lidar coordinate system;

[0167] S3. Based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement, establish an overdetermined linear equation set about the forklift steering wheel and fork arm wheel assembly, and obtain the forklift internal reference value by solving the overdetermined linear equation set using the least squares method.

[0168] S4. Based on the steering wheel data and laser data during forklift movement, determine the forklift transformation relationship and the lidar transformation relationship, and construct an error function based on the forklift transformation relationship and the lidar transformation relationship. Then, optimize the error function based on the forklift internal parameter ratio to obtain the forklift internal parameter value and the lidar external parameter value.

[0169] S5. Calibrate the forklift's intrinsic parameters and the lidar's extrinsic parameters based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

[0170] The present invention provides a method, device, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters. By having a forklift move at a constant speed along an S-shaped or 8-shaped path, and without the aid of calibration templates or other calibration tools, the system constructs a forklift coordinate system, a LiDAR coordinate system, a forklift transformation relationship, and a LiDAR transformation relationship using steering wheel data and LiDAR data during forklift movement. Furthermore, it establishes an overdetermined linear equation set between the forklift steering wheel and the fork arm wheel assembly using the steering wheel data and the LiDAR coordinate system. The system then calculates the forklift intrinsic parameter ratio using the least squares method, obtaining an accurate intrinsic parameter ratio in a short time. Finally, it constructs an error function using the forklift transformation relationship and the LiDAR transformation relationship, and calculates the forklift intrinsic and extrinsic parameters based on the forklift intrinsic parameter ratio, thereby calibrating the forklift's intrinsic and extrinsic parameters and completing the calibration of the 2D LiDAR and the forklift's extrinsic parameters on the unmanned forklift.

[0171] In this embodiment of the invention, the step of establishing an overdetermined linear equation system concerning the forklift steering wheel and fork arm wheel assembly based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement specifically involves:

[0172] The angular velocity and yaw angle of the steering wheel around the center of the steering wheel are obtained based on the steering wheel data when the forklift moves, and the rotation angle of the lidar coordinate system is obtained based on the transformation of the lidar coordinate system.

[0173] A first matrix is ​​constructed based on the angular velocity of the steering wheel around the center of the steering wheel, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift is moving, and an overdetermined linear equation system related to the first matrix is ​​established.

[0174] This invention establishes an overdetermined linear equation system by considering the angular velocity of the steering wheel around its center, the steering wheel yaw angle, and the rotation angle of the lidar coordinate system during forklift movement. This system can be used to solve highly complex optimization problems, and even nonlinear problems, possessing more variables and constraints than typical linear equations. It can solve complex forklift trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0175] In this embodiment of the invention, the overdetermined linear equation system is in the form AX = B;

[0176] Specifically, the overdetermined linear equation system is as follows:

[0177]

[0178] Where, ω s θ is the angular velocity of the steering wheel around its center when the forklift moves, t is time, and θ is the angular velocity of the steering wheel. s For the yaw angle of the steering wheel, This represents the rotation angle between adjacent frames in the lidar coordinate system.

[0179] The embodiments of the present invention establish an overdetermined linear equation system by using the angular velocity of the steering wheel around the center of the steering wheel, the rotation angle of the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift is moving. This system can solve the complex problem of forklift motion trajectory optimization and better adapt to the ever-changing forklift motion trajectory.

[0180] In this embodiment of the invention, obtaining the forklift internal reference value by solving the overdetermined linear equations using the least squares method specifically involves:

[0181] The overdetermined linear equations are transformed using the least squares method to obtain a modified formula. Solving the modified formula yields the forklift internal reference ratio value:

[0182]

[0183] Where, ΔT k For a given time interval [t] k , t k+1 ], For time interval [t] k , t k+1When the forklift moves within the [location], the angular velocity of the steering wheel around its center is [missing value]. For time interval [t] k , t k+1 The yaw angle of the steering wheel within [the area]. For time interval [t] k , t k+1 The rotation angle of adjacent frames in the lidar coordinate system within the [].

[0184] The embodiments of the present invention use the least squares method to transform and solve the existing equations of the upward trend, which can minimize the error, effectively solve complex nonlinear equations so that all constraints are satisfied, and effectively obtain the optimal solution.

[0185] In this embodiment of the invention, the deformed formula is in the form of x = (A T A) -1 A T B.

[0186] The embodiments of the present invention use the least squares method to transform and solve the existing equations of the upward trend, which can minimize the error, effectively solve complex nonlinear equations so that all constraints are satisfied, and effectively obtain the optimal solution.

[0187] In this embodiment of the invention, the step of constructing an error function based on the forklift transformation relationship and the lidar transformation relationship specifically involves:

[0188] An error function is constructed by analyzing the transformation relationships of forklifts and lidar.

[0189] The error function is:

[0190]

[0191] in, Let s be the error value. k For the transformation relationship of lidar, r k For forklift transformation relationship, This is the extrinsic parameter of the lidar coordinate system in the forklift coordinate system.

[0192] This invention transforms the intrinsic and extrinsic parameter calibration problem into an optimization problem by constructing an error function, and then finds the maximum value of this error function. Since the forklift transformation relationship is theoretically derived based on the forklift body model and the integral of the main steering wheel angular velocity, it is a function containing forklift body parameters, i.e., the intrinsic parameters of the forklift body. In practical applications, 2D LiDAR data also contains a certain amount of noise, making it impossible to obtain ideally accurate values ​​for estimating the LiDAR transformation relationship, thus leading to error accumulation. The smaller the accumulated error, the closer the corresponding sensor extrinsic parameters are to the true value.

[0193] In this embodiment of the invention, the step of subsequently calculating the forklift's intrinsic and extrinsic parameters based on the forklift's intrinsic parameter ratio using an error function specifically involves:

[0194] The error function is optimized based on the forklift internal reference value, and transformed into a corresponding quadratic problem. The Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is constructed, and the corresponding homogeneous linear equation system is extracted from it.

[0195] Solve the corresponding homogeneous linear equation system to obtain the first solution vector and the second solution vector;

[0196] By combining the corresponding homogeneous linear equation system with the quadratic problem, and substituting the first solution vector and the second solution vector into the error function, the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value is taken.

[0197] The intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR are calculated based on the position matrix value of the LiDAR coordinate system with the smallest function value in the forklift coordinate system.

[0198] The embodiments of the present invention construct homogeneous linear equations, which can be used to solve for unknown variables in the sparse matrix of forklift motion data that are closest to a specific target value. They can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0199] In this embodiment of the invention, the optimization of the error function based on the forklift internal reference value is transformed into a corresponding quadratic problem. The Lagrangian function of the position matrix of the LiDAR coordinate system in the forklift coordinate system is constructed, and the corresponding homogeneous linear equation system is extracted from it. Specifically:

[0200] The error function is optimized by combining the forklift internal reference ratio as follows:

[0201]

[0202] Transform into the corresponding quadratic form problem The corresponding quadratic form problem is:

[0203]

[0204]

[0205]

[0206] The Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is constructed using Lagrange multiplication, wherein the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is:

[0207]

[0208] The Lagrange function is transformed, and according to... The physical meaning is extracted to obtain the corresponding homogeneous linear equation system, which is:

[0209]

[0210] in, Qk is the construction matrix for the quadratic form problem at time k. Let C be the vector combining the distance from the forklift steering wheel to the forklift arm assembly and the pose of the LiDAR coordinate system in the forklift coordinate system, where C is a constant independent of the optimization variables. x Let l be the position of the lidar coordinate system under the X-axis of the vehicle coordinate system. y Let l be the position of the lidar coordinate system under the vehicle coordinate system axis. θ Let w be the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise; w is a matrix.

[0211] The embodiments of the present invention construct homogeneous linear equations, which can be used to solve for unknown variables in the sparse matrix of forklift motion data that are closest to a specific target value. They can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0212] In this embodiment of the invention, the transformation of the Lagrange function specifically involves:

[0213] By differentiating and transforming the Lagrange function, we obtain the following formula:

[0214]

[0215] in, Let λ be the difference, and λ be the root of the required solution.

[0216] In this embodiment of the invention, the Lagrange function is differentiated and transformed to obtain the corresponding homogeneous linear equation system.

[0217] In this embodiment of the invention, the step of combining the corresponding homogeneous linear equation system with the quadratic problem, substituting the first solution vector and the second solution vector into the error function respectively, and taking the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value, specifically involves:

[0218] Combining the corresponding homogeneous linear equation system with the quadratic form problem, we obtain the corresponding formula:

[0219]

[0220] Substitute the first and second solution vectors into the error function, and take the position matrix value of the LiDAR coordinate system in the forklift coordinate system that has the smallest function value. The smallest position matrix value of the LiDAR coordinate system in the forklift coordinate system is:

[0221]

[0222] Based on the error function, we can obtain:

[0223]

[0224] in, Let L represent the relevant positions of the LiDAR coordinate system in the forklift coordinate system, where L is the distance from the steering wheel to the forklift wheel assembly. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0225] In this embodiment of the invention, the position matrix value of the lidar coordinate system in the forklift coordinate system is obtained by solving the error function, so as to further calculate the intrinsic and extrinsic parameters of the forklift.

[0226] In this embodiment of the invention, the calculation of the forklift intrinsic parameters and the lidar extrinsic parameters based on the position matrix value of the lidar coordinate system with the smallest function value in the forklift coordinate system specifically involves:

[0227] The intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR are calculated based on the position matrix value of the LiDAR coordinate system in the forklift coordinate system, which has the smallest function value. The intrinsic and extrinsic parameters are as follows:

[0228]

[0229]

[0230]

[0231]

[0232]

[0233] The intrinsic parameter value is the steering wheel radius r. s The distance L from the steering wheel to the forklift wheel assembly, and the extrinsic parameter value is the pose of the 2D LiDAR in the forklift body coordinate system. Among them, l xIt is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0234] In this embodiment of the invention, the intrinsic and extrinsic parameters of the forklift are calculated using the position matrix value of the smallest lidar coordinate system in the forklift coordinate system.

[0235] In one embodiment of the present invention, the goods handled by the forklift are generally pallets or cages. Please refer to [link / reference needed]. Figures 2 to 3 The system uses a 2D lidar mounted on top of the autonomous forklift to emit lasers and collect data during its movement. (See also: [link to relevant documentation]). Figure 4 The entire process requires close coordination between the 2D lidar, the main steering wheel, and the end effector of the fork arm. Please refer to [link / reference needed]. Figure 5 Finally, a coordinate system for the motion trajectory is constructed.

[0236] Please see Figure 6 The world frame represents the fixed world coordinate system, the forklift frame represents the body coordinate system that moves with the unmanned forklift, and the lidar frame represents the 2D lidar coordinate system of the lidar installed on the unmanned forklift. This represents the pose of the 2D LiDAR coordinate system in the forklift vehicle coordinate system, i.e. l x and l y It is the position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system (counterclockwise rotation is positive). It is fixed and unchanging. q k q represents the pose of the forklift body coordinate system in the world coordinate system at time k. k+1 r represents the pose of the forklift body coordinate system in the world coordinate system at time k+1. k This represents the transformation relationship of the forklift coordinate system between two adjacent states before and after the unmanned forklift moves. k This represents the transformation relationship of the 2D LiDAR coordinate system between two adjacent states before and after the 2D LiDAR moves while following the forklift. It can be seen that r k The value s can be estimated by integrating the angular velocity of the main steering wheel of the unmanned forklift. k The pose can be estimated using the adjacent frame pose calculation method of 2D LiDAR. The unknown parameters to be determined are the extrinsic parameters of the LiDAR in the forklift body coordinate system, and are part of the parameters to be determined.

[0237] Therefore, assuming a point P in space has coordinates p in the lidar coordinate system at time k+1, then the coordinates of the forklift body in the forklift body coordinate system at time k+1 are: in Representing transformation relationship If applied to p, then the coordinates of the forklift in the forklift body coordinate system at time k are: Similarly, considering another path, the coordinates of P in the lidar coordinate system at time k are expressed as follows: Then the coordinates of P in the forklift body coordinate system at time k are: Therefore, theoretically it should exist. Therefore, Right now in express The effect of the inverse transformation on Above. Ideally, for all time intervals within the entire time period, there is...

[0238]

[0239] Therefore,

[0240]

[0241] However, r k It is derived theoretically based on the forklift body model and the integral of the main steering wheel angular velocity, and is a function that includes forklift body parameters, i.e., r. k =r k (θ1, θ2, ..., θ) N ), θ1, θ2, ..., θ N This represents the forklift vehicle body parameters, i.e., the intrinsic parameters of the forklift vehicle body. In practical applications, 2D LiDAR data will also contain a certain amount of noise, affecting the s k Even estimation cannot yield an ideal, accurate value; the estimated value can be obtained. therefore This constitutes the accumulation of error. The smaller the accumulated error, the lower the corresponding sensor extrinsic parameters. and θ1, θ2, ..., θ N The closer it gets to the true value, the better. The error function for this problem is constructed as follows:

[0242]

[0243] Finding the maximum value of the error accumulation function transforms the calibration problem into an optimization problem. In this application, the forklift vehicle's internal parameter is the radius r of the forklift steering wheel. s The distance L from the steering wheel to the fork arm wheel assembly, although these two parameters have corresponding theoretical values ​​for machining and assembly, are considered as the second part of the parameters to be solved here due to errors in machining and assembly.

[0244] Based on the mathematical model of a single-steering wheel unmanned forklift, the trajectory derivation of the integral of the angular velocity of the forklift's main steering wheel is derived.

[0245] Please refer to the poses of the unmanned forklift at time k and time k+1. Figure 7 .

[0246] ω s v s and r s These represent the angular velocities of the steering wheel around its center during the movement of the unmanned forklift (the number of revolutions n within a time interval Δt, calculated using the following formula). ), linear velocity and radius, and v s =ω s r s .

[0247] When the unmanned forklift is in motion, its steering is controlled by the steering wheel. When the steering wheel yaw angle is θ... s At that time, the forklift will move around a point O in the two-dimensional space. c It moves in a circular motion. ω IF v IF and r represent the midpoint O of the line connecting the two wheels of the forklift arm during forklift movement (this point is usually chosen as the reference point for forklift movement), and the points around O. c The angular velocity (rotation angle r within the time interval Δt) θ The formula is ), linear velocity and radius of rotation, and v IF =ω IF r, when ω IF When ω = 0, the forklift travels in a straight line. d This indicates that the steering wheel of the forklift revolves around O during its movement. c angular velocity,

[0248] According to the principles of kinematics,

[0249] v IF =r s ω s cos(θ s )

[0250] Based on geometric relationships, L is the distance from the center of the main steering wheel to the geometric center of the wishbone wheel assembly.

[0251]

[0252] but

[0253]

[0254] Written in matrix form, we have

[0255]

[0256] Let J1 = r s and

[0257]

[0258]

[0259] According to equation (2), ω IF =J2ω s sin(θ s The unmanned forklift moves from time k to time k+1, with...

[0260]

[0261] According to the kinematic model, therefore

[0262]

[0263] visible The relationship between J2 and the coordinate system is linear. Since the lidar is fixed on the unmanned forklift, the rotation angle of the unmanned forklift's coordinate system from time k to time k+1... Angle relative to the lidar coordinate system Old etc., that is Therefore, equation (3) can be extended to

[0264]

[0265]

[0266] ...

[0267]

[0268] Further written in matrix form is

[0269]

[0270] It can be seen that equation (4) is an overdetermined linear system of equations in J2, in the form AX = B, which can be solved by the least squares method to find x = (A T A) -1 A T B. This corresponds to... Estimated as

[0271]

[0272] In practical applications, ω s (t) and θs (t) at a given time interval [t k , t k+1 The value within ΔT is usually considered a constant. k =t k+1 -t k Therefore, it can be written as

[0273]

[0274] Because of the estimated parameters If there is only one value, then

[0275]

[0276] In summary, it can be estimated that The value of .

[0277] because It is evident that r can be obtained simply by estimating the value of L. s Equation (1) can be transformed into in and The physical meaning is the distance the forklift travels in the X and Y directions from time k to time k+1.

[0278]

[0279]

[0280] in The physical meaning is the magnitude of the rotation angle of the forklift from time k to time k+1.

[0281]

[0282] According to equation (2), v IF =J1ω s cos(θ s ), and J1 = LJ2, then v IF =L(J2ω) s cos(θ s )).therefore

[0283]

[0284]

[0285] In practical applications, ω s (t) and θ s (t) at a given time interval [t k , tk+1 [The text is incomplete and appears to be a fragment of a larger document. A more accurate translation would require the full context.] and ΔT k =t k+1 -t k Therefore, there is

[0286]

[0287]

[0288]

[0289] in and

[0290] therefore,

[0291] And loss function

[0292]

[0293] in It is a constant independent of the optimization variable. Therefore, optimize the loss function. This is equivalent to optimizing a quadratic form problem. Right now

[0294]

[0295] To obtain a closed-form solution, Lagrange multiplication is used. The constraints are... Transformed into

[0296]

[0297] Construct the Lagrange function as follows but

[0298]

[0299] according to The physical meaning of homogeneous linear equations There should exist non-zero solutions, that is... Since λ is the kernel of the coefficient matrix (M+λW), λ can be solved using det(M+λW)=0.

[0300]

[0301] Note the element in the 1st row and 2nd column, according to... and have

[0302]

[0303] therefore

[0304]

[0305] Therefore,

[0306]

[0307] Since λ exists only at two diagonals, det(M+λW)=0 is a quadratic equation in λ, i.e., a²λ. 2 +a1λ+a0=0, according to the rules of determinant calculation, we can calculate:

[0308]

[0309]

[0310]

[0311] This allows us to solve the two roots λ of the quadratic equation. (1) and λ (2) According to the physical meaning, the rank of the coefficient matrix (M+λW) is 4, corresponding to a 1-dimensional solution space. Let λ... (1) Substitute into the nonzero linear equation system The corresponding solution vector can be obtained as ψ. (1) , will λ (2) Substitute into the nonzero linear equation system The corresponding solution vector can be obtained as ψ. (2) If we remove the scale constraint from the constraints in equation (7), then we have

[0312]

[0313] Then Substitute the values ​​into the objective function and take the corresponding value that minimizes the objective function value.

[0314] The previous estimates have already been made. Then the robot's intrinsic and extrinsic parameters can be further estimated as follows:

[0315]

[0316]

[0317]

[0318]

[0319]

[0320] As mentioned above, the intrinsic parameter in this invention is the rudder radius r. s The distance L from the steering wheel to the forklift wheel assembly is given by the extrinsic parameter, which is the pose l of the 2D LiDAR in the forklift body coordinate system. x and l y It is the position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system (counterclockwise rotation is positive).

[0321] This invention calibrates the extrinsic parameters of the 2D LiDAR and the forklift itself, as well as the intrinsic parameters of the forklift's steering wheel radius and the distance from the steering wheel to the fork arm wheel assembly, using only a simple operation such as driving an "8" or "S" shaped trajectory on a single-steering-wheel unmanned forklift, without the aid of calibration templates or other calibration tools. The calibration method is simple, has low requirements for the calibration environment, and therefore has low calibration costs and is easy to mass-produce.

[0322] Please see Figure 8 The present invention also proposes a joint calibration device based on lidar and forklift internal and external parameters, including an acquisition module 1, a construction module 2, a solution module 3, a calculation module 4 and a calibration module 5;

[0323] The acquisition module 1 is used to control the forklift to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and laser data recorded in real time by the lidar when the forklift is moving.

[0324] The construction module 2 is used to construct the forklift coordinate system and the lidar coordinate system;

[0325] The solution module 3 establishes an overdetermined linear equation set about the forklift steering wheel and fork arm wheel assembly based on the transformation of the steering wheel data and the lidar coordinate system when the forklift moves, and obtains the forklift internal reference value by solving the overdetermined linear equation set using the least squares method.

[0326] The calculation module 4 is used to determine the forklift transformation relationship and the lidar transformation relationship based on the steering wheel data and the laser data when the forklift is moving, and to construct an error function based on the forklift transformation relationship and the lidar transformation relationship. Then, it optimizes the error function based on the forklift internal parameter ratio to obtain the forklift internal parameter value and the lidar external parameter value.

[0327] The calibration module 5 is used to calibrate the forklift's intrinsic parameters and the lidar's extrinsic parameters based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

[0328] This invention employs an acquisition module to instruct the forklift to move at a constant speed along an S-shaped or 8-shaped path. Without the aid of calibration templates or other calibration tools, it acquires steering wheel data and laser data during forklift movement. A construction module establishes forklift coordinate systems, a laser radar coordinate system, forklift transformation relationships, and laser radar transformation relationships. A solution module further establishes overdetermined linear equations for the forklift steering wheel and fork arm wheel assembly in both coordinate systems. The least squares method is used to calculate the forklift's intrinsic parameter ratio, obtaining accurate ratios in a short time. A calculation module calculates the forklift and laser radar transformation relationships to further construct an error function. Combined with the forklift intrinsic parameter ratio, the intrinsic and extrinsic parameters of the forklift are calculated for calibration. Finally, a calibration module completes the calibration of the 2D laser radar and the extrinsic parameters of the forklift itself on the unmanned forklift.

[0329] Please see Figure 8 In this embodiment of the invention, the solving module 3 includes a first unit and a second unit;

[0330] The first unit is used to obtain the angular velocity of the steering wheel around the center of the steering wheel and the yaw angle of the steering wheel when the forklift moves, based on the steering wheel data when the forklift moves, and to obtain the rotation angle of the lidar coordinate system based on the transformation of the lidar coordinate system.

[0331] The second unit is used to construct a first matrix based on the angular velocity of the steering wheel around the center of the steering wheel, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift moves, and to establish an overdetermined linear equation system related to the first matrix.

[0332] This invention establishes an overdetermined linear equation system by considering the angular velocity of the steering wheel around its center, the rotation angle in the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle in the lidar coordinate system during forklift movement. This system can be used to solve highly complex optimization problems, and even nonlinear problems, possessing more variables and constraints than typical linear equations. It can solve complex forklift trajectory optimization problems and better adapt to changing forklift movement trajectories.

[0333] Please see Figure 8 In this embodiment of the invention, the overdetermined linear equation system is in the form AX = B;

[0334] Specifically, the overdetermined linear equation system is as follows:

[0335]

[0336] Where, ω s θ is the angular velocity of the steering wheel around its center when the forklift moves, t is time, and θ is the angular velocity of the steering wheel. s For the yaw angle of the steering wheel, This represents the rotation angle between adjacent frames in the lidar coordinate system.

[0337] The embodiments of the present invention establish an overdetermined linear equation system by using the angular velocity of the steering wheel around the center of the steering wheel, the rotation angle of the forklift coordinate system, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift is moving. This system can solve the complex problem of forklift motion trajectory optimization and better adapt to the ever-changing forklift motion trajectory.

[0338] Please see Figure 8 In this embodiment of the invention, the solving module 3 further includes a third unit and a fourth unit;

[0339] The third unit is used to transform the overdetermined linear equations using the least squares method to obtain a deformed formula, and then solves the deformed formula to obtain the forklift internal reference value:

[0340]

[0341] Where, ΔT k For a given time interval [t] k , t k+1 ], For time interval [t] k , t k+1 When the forklift moves within the [location], the angular velocity of the steering wheel around its center is [missing value]. For time interval [t] k , t k+1 The yaw angle of the steering wheel within [the area]. For time interval [t] k , t k+1 The rotation angle of adjacent frames in the lidar coordinate system within the [].

[0342] The embodiments of the present invention use the least squares method to transform and solve the existing equations of the upward trend, which can minimize the error, effectively solve complex nonlinear equations so that all constraints are satisfied, and effectively obtain the optimal solution.

[0343] Please see Figure 8 In this embodiment of the invention, the transformed formula is in the form of x = (A T A) -1 A T B.

[0344] The embodiments of the present invention use the least squares method to transform and solve the existing equations of the upward trend, which can minimize the error, effectively solve complex nonlinear equations so that all constraints are satisfied, and effectively obtain the optimal solution.

[0345] Please see Figure 8 In this embodiment of the invention, the computing module 4 includes a fifth unit;

[0346] The fifth unit is used to construct an error function based on the forklift transformation relationship and the lidar transformation relationship;

[0347] The error function is:

[0348]

[0349] in, Let s be the error value. k For the transformation relationship of lidar, r k For forklift transformation relationship, This is the extrinsic parameter of the lidar coordinate system in the forklift coordinate system.

[0350] This invention transforms the intrinsic and extrinsic parameter calibration problem into an optimization problem by constructing an error function, and then finds the maximum value of this error function. Since the forklift transformation relationship is theoretically derived based on the forklift body model and the integral of the main steering wheel angular velocity, it is a function containing forklift body parameters, i.e., the intrinsic parameters of the forklift body. In practical applications, 2D LiDAR data also contains a certain amount of noise, making it impossible to obtain ideally accurate values ​​for estimating the LiDAR transformation relationship, thus leading to error accumulation. The smaller the accumulated error, the closer the corresponding sensor extrinsic parameters are to the true value.

[0351] Please see Figure 8 In this embodiment of the invention, the calculation module 4 further includes a sixth unit, a seventh unit, an eighth unit, and a ninth unit;

[0352] The sixth unit is used to optimize the error function based on the forklift internal reference value, convert it into a corresponding quadratic problem, construct the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system, and extract the corresponding homogeneous linear equation system from it.

[0353] The seventh unit is used to solve the corresponding homogeneous linear equation system to obtain the first solution vector and the second solution vector.

[0354] The eighth unit is used to combine the corresponding homogeneous linear equation system with the quadratic problem, and substitute the first solution vector and the second solution vector into the error function respectively, and take the position matrix value of the lidar coordinate system in the forklift coordinate system with the smallest function value.

[0355] The ninth unit is used to calculate the intrinsic parameters of the forklift and the extrinsic parameters of the lidar based on the position matrix value of the lidar coordinate system with the smallest function value in the forklift coordinate system.

[0356] The embodiments of the present invention construct homogeneous linear equations, which can be used to solve for unknown variables in the sparse matrix of forklift motion data that are closest to a specific target value. They can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0357] Please see Figure 8 In this embodiment of the invention, the sixth unit includes a first subunit, a second subunit, a third subunit, and a fourth subunit;

[0358] The first subunit is used to optimize the error function by combining the forklift internal reference ratio:

[0359]

[0360] The second sub-unit is used to transform the problem into the corresponding quadratic form. The corresponding quadratic form problem is:

[0361]

[0362]

[0363]

[0364] The third subunit is used to construct the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system using Lagrange multiplication, wherein the Lagrangian function of the position matrix of the lidar coordinate system in the forklift coordinate system is:

[0365]

[0366] The fourth sub-unit is used to transform the Lagrange function, and according to... The physical meaning is extracted to obtain the corresponding homogeneous linear equation system, which is:

[0367]

[0368] in, Q k Let k be the construction matrix for the quadratic form problem at time k. Let C be the vector combining the distance from the forklift steering wheel to the forklift arm assembly and the pose of the LiDAR coordinate system in the forklift coordinate system, where C is a constant independent of the optimization variables. x Let l be the position of the lidar coordinate system under the x-axis of the vehicle coordinate system. y Let l be the position of the lidar coordinate system under the y-axis of the vehicle coordinate system. θLet w be the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise; w is a matrix.

[0369] The embodiments of the present invention construct homogeneous linear equations, which can be used to solve for unknown variables in the sparse matrix of forklift motion data that are closest to a specific target value. They can also be used to determine the relationship between any forklift motion data points and to derive the optimal solution for any unknown variable, thus providing convenient and effective conditions.

[0370] Please see Figure 8 In this embodiment of the invention, the fourth subunit includes a first submodule;

[0371] The first submodule is used to differentiate and transform the Lagrange function to obtain the formula:

[0372]

[0373] in, Let λ be the difference, and λ be the root of the required solution.

[0374] In this embodiment of the invention, the Lagrange function is differentiated and transformed to obtain the corresponding homogeneous linear equation system.

[0375] Please see Figure 8 In this embodiment of the invention, the eighth unit includes a fifth subunit, a sixth subunit, and a seventh subunit;

[0376] The fifth subunit is used to combine the corresponding homogeneous linear equation system with the quadratic problem to obtain the corresponding formula:

[0377]

[0378] The sixth subunit is used to substitute the first and second solution vectors into the error function, and take the position matrix value of the LiDAR coordinate system in the forklift coordinate system with the smallest function value. The smallest position matrix value of the LiDAR coordinate system in the forklift coordinate system is:

[0379]

[0380] The seventh sub-unit is used to obtain the following from the error function:

[0381]

[0382] in, Let L represent the relevant positions of the LiDAR coordinate system in the forklift coordinate system, where L is the distance from the steering wheel to the forklift wheel assembly. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l yIt is the Y-axis position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0383] In this embodiment of the invention, the position matrix value of the lidar coordinate system in the forklift coordinate system is obtained by solving the error function, so as to further calculate the intrinsic and extrinsic parameters of the forklift.

[0384] Please see Figure 8 In this embodiment of the invention, the ninth unit includes an eighth subunit;

[0385] The eighth subunit is used to calculate the intrinsic parameters of the forklift and the extrinsic parameters of the LiDAR based on the position matrix value of the LiDAR coordinate system in the forklift coordinate system with the smallest function value. The intrinsic and extrinsic parameters are as follows:

[0386]

[0387]

[0388]

[0389]

[0390]

[0391] The intrinsic parameter value is the steering wheel radius r. s The distance L from the steering wheel to the forklift wheel assembly is given by the extrinsic parameter l, which represents the pose of the 2D LiDAR in the forklift's body coordinate system. x It is the X-axis position of the lidar coordinate system in the vehicle coordinate system, l y It is the Y-axis position of the lidar coordinate system in the vehicle coordinate system, l θ It is the angle between the X-axis of the LiDAR coordinate system and the X-axis of the forklift coordinate system, with a positive value when rotated counterclockwise.

[0392] In this embodiment of the invention, the intrinsic and extrinsic parameters of the forklift are calculated using the position matrix value of the smallest lidar coordinate system in the forklift coordinate system.

[0393] The embodiment of the joint calibration device based on lidar and forklift internal and external parameters provided by the present invention can be used to implement the process steps in the above embodiments.

[0394] In this embodiment of the invention, a joint calibration system based on lidar and forklift intrinsic and extrinsic parameters is proposed, including a forklift, lidar, and computing device;

[0395] The forklift is used to carry the lidar and move at a constant speed along an S-shaped or 8-shaped route;

[0396] The lidar is used to emit lasers and record laser data in real time;

[0397] The computing device is used to execute the above-mentioned joint calibration method based on lidar and forklift intrinsic and extrinsic parameters.

[0398] In this embodiment of the invention, a forklift moves at a constant speed along an S-shaped or 8-shaped route; a lidar emits laser light and records laser data in real time; without the aid of calibration templates or other calibration tools, a computing device constructs a forklift coordinate system, a lidar coordinate system, a forklift transformation relationship, and a lidar transformation relationship using the steering wheel data and laser data during forklift movement. The forklift coordinate system and the lidar coordinate system are further used to establish an overdetermined linear equation set between the forklift steering wheel and the fork arm wheel assembly. The forklift's internal parameter ratio is calculated using the least squares method, obtaining an accurate internal parameter ratio in a short time. An error function is further constructed using the forklift transformation relationship and the lidar transformation relationship, and the forklift's internal and external parameter values ​​are calculated in conjunction with the internal parameter ratio to calibrate the forklift's internal and external parameters, thus completing the calibration of the 2D lidar and the forklift's external parameters on the unmanned forklift.

[0399] This invention provides a method, device, and system for joint calibration of LiDAR and forklift intrinsic and extrinsic parameters. By having the forklift move at a constant speed along an S-shaped or 8-shaped path, data from the steering wheel and LiDAR are acquired during the forklift's movement without the aid of calibration templates or other calibration tools. A forklift coordinate system, a LiDAR coordinate system, forklift transformation relationships, and LiDAR transformation relationships are constructed. Furthermore, overdetermined linear equations for the forklift steering wheel and fork arm wheel assembly are established for the forklift and LiDAR coordinate systems. The least squares method is used to calculate the forklift intrinsic parameter ratio, obtaining an accurate intrinsic parameter ratio in a short time. The forklift transformation relationships and LiDAR transformation relationships are calculated to further construct an error function. Combined with the forklift intrinsic parameter ratio, the intrinsic and extrinsic parameters of the forklift are calculated for calibration, ultimately completing the calibration of the 2D LiDAR and the forklift's extrinsic parameters on the unmanned forklift.

[0400] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A method for joint calibration of lidar and forklift intrinsic and extrinsic parameters, characterized in that, include: The forklift is controlled to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and real-time laser data recorded by the lidar during the forklift's movement. Construct the forklift coordinate system and the lidar coordinate system; Based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement, an overdetermined linear equation system regarding the forklift steering wheel and fork arm wheel assembly is established. The forklift's internal reference values ​​are obtained by solving the overdetermined linear equation system using the least squares method. The form of the overdetermined linear equation system is as follows: ; Specifically, the overdetermined linear equation system is as follows: in, The angular velocity of the steering wheel around its center when the forklift is in motion. For time, For the yaw angle of the steering wheel, The rotation angle between adjacent frames in the lidar coordinate system; Based on the steering wheel data and laser data during forklift movement, the forklift transformation relationship and the lidar transformation relationship are determined. An error function is constructed using the forklift transformation relationship and the lidar transformation relationship. Then, the error function is optimized based on the forklift intrinsic parameter ratio to obtain the forklift intrinsic parameter value and the lidar extrinsic parameter value. The error function is: in, This is the error value. For lidar transformation relationships, Forklift transformation relationship, The external parameters of the lidar coordinate system in the forklift coordinate system; The forklift's intrinsic parameters and the lidar's extrinsic parameters are calibrated based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

2. The method for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to claim 1, characterized in that, The process involves establishing an overdetermined linear equation system regarding the forklift's steering wheel and fork arm wheel assembly based on the transformation of the steering wheel data and the lidar coordinate system during forklift movement. Specifically: The angular velocity and yaw angle of the steering wheel around the center of the steering wheel are obtained based on the steering wheel data when the forklift moves, and the rotation angle of the lidar coordinate system is obtained based on the transformation of the lidar coordinate system. A first matrix is ​​constructed based on the angular velocity of the steering wheel around the center of the steering wheel, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift is moving, and an overdetermined linear equation system related to the first matrix is ​​established.

3. The method for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to claim 2, characterized in that, The forklift internal reference values ​​are obtained by solving the overdetermined linear equations using the least squares method, specifically as follows: The overdetermined linear equations are transformed using the least squares method to obtain a modified formula. Solving the modified formula yields the forklift internal reference ratio value: in, For a given time interval , Time interval The angular velocity of the steering wheel around its center when the forklift is in motion. Time interval Yaw angle of the steering wheel Time interval The rotation angle between adjacent frames in the lidar coordinate system.

4. The method for joint calibration of lidar and forklift intrinsic and extrinsic parameters according to claim 3, characterized in that, The form of the deformation formula is: .

5. A calibration device based on lidar and forklift internal and external parameters, characterized in that, It includes an acquisition module, a construction module, a solution module, a calculation module, and a calibration module; The acquisition module is used to control the forklift to move at a constant speed along an S-shaped or 8-shaped route. The forklift is equipped with a lidar to acquire steering wheel data and laser data recorded in real time by the lidar when the forklift is moving. The construction module is used to construct the forklift coordinate system and the lidar coordinate system; The solution module establishes an overdetermined linear equation system regarding the forklift's steering wheel and fork arm wheel assembly based on the data from the steering wheel and the transformation of the lidar coordinate system during forklift movement. It then solves this overdetermined linear equation system using the least squares method to obtain the forklift's internal reference values. The form of the overdetermined linear equation system is as follows: ; Specifically, the overdetermined linear equation system is as follows: in, The angular velocity of the steering wheel around its center when the forklift is in motion. For time, For the yaw angle of the steering wheel, The rotation angle between adjacent frames in the lidar coordinate system; The calculation module is used to determine the forklift transformation relationship and the lidar transformation relationship based on the steering wheel data and the laser data when the forklift is moving, and to construct an error function based on the forklift transformation relationship and the lidar transformation relationship. Then, the error function is optimized based on the forklift internal parameter ratio to obtain the forklift internal parameter value and the lidar external parameter value. The error function is: in, This is the error value. For lidar transformation relationships, For forklift transformation relationship, The external parameters of the lidar coordinate system in the forklift coordinate system; The calibration module is used to calibrate the forklift's intrinsic parameters and the lidar's extrinsic parameters based on the forklift's intrinsic parameters and the lidar's extrinsic parameters.

6. The calibration device based on lidar and forklift intrinsic and extrinsic parameters according to claim 5, characterized in that, The solution module includes a first unit and a second unit; The first unit is used to obtain the angular velocity of the steering wheel around the center of the steering wheel and the yaw angle of the steering wheel when the forklift moves, based on the steering wheel data when the forklift moves, and to obtain the rotation angle of the lidar coordinate system based on the transformation of the lidar coordinate system. The second unit is used to construct a first matrix based on the angular velocity of the steering wheel around the center of the steering wheel, the yaw angle of the steering wheel, and the rotation angle of the lidar coordinate system when the forklift moves, and to establish an overdetermined linear equation system related to the first matrix.

7. The calibration device based on lidar and forklift intrinsic and extrinsic parameters according to claim 5, characterized in that, The solution module also includes a third unit and a fourth unit; The third unit is used to transform the overdetermined linear equations using the least squares method to obtain a deformed formula, and then solves the deformed formula to obtain the forklift internal reference value: in, For a given time interval , Time interval The angular velocity of the steering wheel around its center when the forklift is in motion. Time interval yaw angle of the steering wheel Time interval The rotation angle between adjacent frames in the lidar coordinate system.

8. The calibration device based on lidar and forklift internal and external parameters according to claim 7, characterized in that, The form of the deformation formula is: .

9. A joint calibration system based on lidar and forklift intrinsic and extrinsic parameters, characterized in that, This includes forklifts, lidar, and computing devices; The forklift is used to carry the lidar and move at a constant speed along an S-shaped or 8-shaped route; The lidar is used to emit lasers and record laser data in real time; The computing device is used to execute any one of the calibration methods of LiDAR and forklift intrinsic and extrinsic parameters according to claims 1 to 4.

Images