WO2023131123A1 - 组合导航设备与激光雷达的外参标定方法及装置 - Google Patents

组合导航设备与激光雷达的外参标定方法及装置 Download PDF

Info

Publication number
WO2023131123A1
WO2023131123A1 PCT/CN2023/070132 CN2023070132W WO2023131123A1 WO 2023131123 A1 WO2023131123 A1 WO 2023131123A1 CN 2023070132 W CN2023070132 W CN 2023070132W WO 2023131123 A1 WO2023131123 A1 WO 2023131123A1
Authority
WO
WIPO (PCT)
Prior art keywords
point
pose
original
external parameter
navigation device
Prior art date
Application number
PCT/CN2023/070132
Other languages
English (en)
French (fr)
Inventor
张敏
Original Assignee
上海三一重机股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 上海三一重机股份有限公司 filed Critical 上海三一重机股份有限公司
Publication of WO2023131123A1 publication Critical patent/WO2023131123A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Definitions

  • the present application relates to the technical field of intelligent driving, in particular to a method and device for calibrating external parameters of integrated navigation equipment and laser radar.
  • High-precision integrated navigation equipment and laser radar are commonly used sensors in the high-precision positioning and perception technology of operating machinery. It is difficult to obtain external parameter calibration parameters by using external parameter measurement methods for the installation position on the operating machine.
  • the external parameter calibration of integrated navigation equipment and lidar is mainly realized by converting the data collected by lidar into the coordinate system of integrated navigation equipment.
  • external sensors can be used to directly measure external parameters.
  • the direct measurement process is complicated and has large measurement errors.
  • the lidar it is also possible to use the lidar to run the SLAM algorithm to obtain the mileage information of the lidar, and then combine the mileage information calculated by the integrated navigation device itself to calculate the transformation matrix between the two coordinate systems to obtain the external distance between the two sensors.
  • the above method requires a large amount of calculation, takes a long time, and the accuracy of the external parameter calibration results is low.
  • This application provides a method and device for calibrating external parameters of integrated navigation equipment and laser radar, which are used to solve the defects of cumbersome operation, long time-consuming and low accuracy of results in the calibration method of external parameters of integrated navigation equipment and laser radar in the prior art. Realize efficient and accurate external parameter calibration of integrated navigation equipment and lidar.
  • the present application provides a method for calibrating external parameters of a combined navigation device and lidar, the method comprising:
  • the optimal external parameter calibration solution is obtained.
  • the pose interpolation is performed on the original pose data to obtain Pose interpolation results, including:
  • the adjacent pose points adjacent to the timestamp corresponding to each radar point in the original point cloud data are obtained from the original pose data;
  • the initial value of the external parameter is calculated according to the original pose data, the original point cloud data, and the pose interpolation result, including:
  • the transformation point coordinate expression is used to describe that the transformation point coordinate is the pose transformation matrix from the virtual point to the initial moment point of the integrated navigation device , the pre-built external parameter matrix to be sought and the radar point coordinates are calculated.
  • the optimal external parameter calibration solution is obtained by solving, including:
  • an optimization objective function is constructed with the minimum sum of the distances of the closest points in the original point cloud data as the optimization objective, and an optimal external parameter calibration solution is obtained by solving.
  • the optimization objective function is constructed with the minimum sum of the distances of the closest points in the original point cloud data as the optimization goal , to obtain the optimal external parameter calibration solution, including:
  • the currently assigned extrinsic parameter matrix is used as the optimal extrinsic parameter calibration solution.
  • the optimization objective function is constructed with the minimum sum of the distances of the closest points in the original point cloud data as the optimization goal , to obtain the optimal external parameter calibration solution, including:
  • the currently assigned extrinsic parameter matrix is used as the optimal extrinsic parameter calibration solution.
  • an external parameter calibration device for a combined navigation device and laser radar the device includes:
  • the obtaining module is used to obtain the original pose data collected by the integrated navigation device and the original point cloud data collected by the laser radar;
  • a first processing module configured to time-synchronize the original point cloud data and the original pose data, to obtain time-synchronized time stamp information
  • the second processing module is configured to perform pose interpolation on the original pose data according to the time-synchronized timestamp information and the original point cloud data, to obtain a pose interpolation result;
  • a third processing module configured to calculate an initial value of an external parameter according to the original pose data, the original point cloud data, and the pose interpolation result
  • the fourth processing module is used to obtain an optimal external parameter calibration solution based on the initial value of the external parameter and the pre-built optimization objective function.
  • the present application also provides an operation machine, which uses any one of the methods for calibrating external parameters of a combined navigation device and laser radar described above.
  • the present application also provides an external parameter calibration system for a combined navigation device and laser radar, which includes:
  • the laser radar to be calibrated is used to collect the original point cloud data of the target
  • the integrated navigation device to be calibrated is used to collect the original pose data of the target
  • a controller configured to respectively acquire the original pose data and the original point cloud data; time-synchronize the original point cloud data and the original pose data to obtain time-synchronized timestamp information; according to the The timestamp information after the time synchronization and the original point cloud data, perform pose interpolation on the original pose data, and obtain the pose interpolation result; according to the original pose data, the original point cloud data and the position Based on the interpolation results of the external parameters, the initial value of the external parameters is calculated; and based on the initial values of the external parameters and the pre-built optimization objective function, the optimal external parameter calibration solution is obtained by solving.
  • the external parameter calibration method and device of the integrated navigation equipment and laser radar provided by this application can effectively improve the efficiency of obtaining the optimal external parameter calibration solution and the accuracy of the results by obtaining a relatively ideal initial value of the external parameter.
  • it is more convenient and efficient, which in turn reduces the time-consuming of the entire external parameter calibration process, making the external parameter calibration process more efficient. Calibration results are more accurate.
  • Fig. 1 is a schematic flow chart of the external parameter calibration method of the integrated navigation device and laser radar provided by the present application;
  • Fig. 2 is a schematic diagram of the calculation process of the initial value of the external parameter
  • Fig. 3 is a schematic structural diagram of the external reference calibration device of the combined navigation equipment and laser radar provided by the present application;
  • Figure 4 is a schematic diagram of the hardware architecture of the external parameter calibration system built during the implementation process
  • Fig. 5 is a schematic diagram of the workflow of the external reference calibration system
  • FIG. 6 is a schematic structural diagram of an electronic device provided by the present application.
  • Fig. 1 shows the external parameter calibration method of the combined navigation device and laser radar provided by the embodiment of the present application, the method includes:
  • Step 110 Obtain the original pose data collected by the integrated navigation device and the original point cloud data collected by the lidar.
  • the integrated navigation device is mainly aimed at the RTK/IMU integrated navigation device.
  • RTK Real Time Kinematic, real-time dynamic positioning
  • RTK is a satellite navigation technology, which is used to improve the accuracy of the position data obtained from the satellite-based positioning system.
  • Accuracy in addition to the information contained in the signal, RTK also uses the measurement of the phase of the carrier signal and relies on a single reference station or interpolated virtual station to make real-time corrections to provide positioning accuracy up to centimeters.
  • IMU Inertial Measurement Unit, Inertial Measurement Unit
  • IMU Inertial Measurement Unit, Inertial Measurement Unit
  • the RTK/IMU integrated navigation device can acquire satellite navigation data and inertial measurement data at the same time, providing reliable data support for the subsequent external parameter calibration process.
  • the lidar in this embodiment is mainly aimed at 3D lidar.
  • 3D lidar can obtain three-dimensional depth information and point cloud data. It has the advantages of high resolution and low power consumption. It is a relatively common sensing device used on operating machinery.
  • Step 120 Time-synchronize the original point cloud data and the original pose data to obtain time-synchronized time stamp information.
  • the original point cloud data and the original pose data are first time-aligned to realize the time difference compensation of the transmission delay between the laser radar and the integrated navigation device.
  • the time difference to be compensated is set to be ⁇ t 1 .
  • the time difference ⁇ t 1 that needs to be compensated, the time difference ⁇ t 2 between the initial moment of the lidar and the initial moment of the integrated navigation device, the time stamp t h of each scan frame, and the scan frame from each point in each scan frame to the point where the point is located, start time stamp t of , and finally calculate the time stamp t p of each point in each scan frame relative to the initial moment of the integrated navigation device, the calculation formula is as follows:
  • time synchronization process in this embodiment mainly calculates and updates the time stamp of the laser radar, and the purpose is to synchronize the time stamp of the laser radar with the time stamp of the integrated navigation device as much as possible to ensure that the subsequent data processing process is effective. conduct.
  • Step 130 Perform pose interpolation on the original pose data according to the time-synchronized timestamp information and the original point cloud data, to obtain a pose interpolation result.
  • the process of performing pose interpolation on the original pose data according to time-synchronized time stamp information and original point cloud data to obtain a pose interpolation result specifically includes:
  • Step 1 Obtain the pose transformation matrix and corresponding time stamp information of each pose point in the original pose data from the current moment to the initial moment, and obtain the corresponding coordinate system of each radar point at the corresponding moment of the integrated navigation device. virtual point.
  • the pose transformation matrix T_t 0 _t k of each pose point from the current time t k to the initial time t 0 is calculated through the integral transformation of the pose points of the integrated navigation device and the IMU integral transformation.
  • the virtual point corresponding to the radar point in the coordinate system at the corresponding moment of the integrated navigation device can be obtained by multiplying the coordinates of the radar point by the preset external parameter matrix to be obtained.
  • the second step According to the timestamp information after time synchronization, the adjacent pose points adjacent to the timestamp corresponding to each radar point in the original point cloud data are obtained from the original pose data.
  • the two time stamps closest to the time stamp t p in the integrated navigation device are t k and t k+1 and satisfy t k ⁇ t p ⁇ t k+1 , correspondingly, the pose points corresponding to the time t k and t k+1 can be known, that is, the adjacent pose points, and at the same time, it can be known that the current time to The pose transformation matrix T_t 0 _t k at the initial moment, and the pose transformation matrix T_t 0 _t k+1 corresponding to the pose point at the moment t k+ 1 from the current moment to the initial moment.
  • Step 3 Based on the timestamp information of adjacent pose points and the corresponding pose transformation matrix, calculate the pose transformation matrix from the virtual point to the initial moment point of the integrated navigation device as the result of pose interpolation.
  • the pose transformation matrix from the virtual point to the initial moment point of the integrated navigation device in this step that is, the calculation formula of the virtual pose transformation matrix T_t 0 _t p is as follows:
  • the virtual point mentioned in this embodiment refers to the point corresponding to the time t p in the integrated navigation coordinate system, because this point does not actually exist in the integrated navigation coordinate system , so this point is called a virtual point.
  • Step 140 According to the original pose data, the original point cloud data and the pose interpolation result, calculate the initial value of the extrinsic parameters.
  • a matrix of external parameters to be obtained is constructed in advance in this embodiment, and the matrix of external parameters to be obtained includes rotation angle and translation amount.
  • the external parameter matrix to be found can specifically include three rotation angles and three translation amounts.
  • the rotation angle can be heading angle ⁇ , roll angle ⁇ , and yaw angle ⁇
  • the translation amount can be the translation amount b on the X coordinate axis. 1.
  • the external parameter matrix T to be obtained can be expressed as:
  • R represents the rotation matrix
  • t represents the translation matrix
  • a 1 to a 9 represent the elements in the rotation matrix
  • b 1 to b 3 represent the elements in the translation matrix.
  • the to-be-required external parameter matrix T includes six unknowns, and the solution result of the to-be-required external parameter matrix can be obtained by solving the above six unknowns, and the external parameter calibration solution can be obtained.
  • the transformation point coordinate expression can be constructed. It is obtained by multiplying the pose transformation matrix at the initial time point, the external parameter matrix to be obtained, and the coordinates of the radar point.
  • the pose transformation matrix from each radar point to the initial moment coordinate system of the integrated navigation device can be obtained by multiplying the corresponding virtual pose transformation matrix by the external parameter matrix to be solved, therefore, when calculating each radar point
  • the coordinates of the corresponding transformation point in the initial time coordinate system of the integrated navigation device can be obtained by multiplying the radar point coordinates to the pose transformation matrix of the initial time coordinate system of the integrated navigation device.
  • T p i represents the virtual point coordinates of the laser radar point p i converted to the coordinate system of the integrated navigation device at the current moment
  • T_t 0 _t p ⁇ T ⁇ p i represents the transformation point coordinates for transforming the lidar point p i into the coordinate system at the initial moment of the integrated navigation device.
  • the process of calculating the initial value of the external parameters according to the original pose data, original point cloud data, and pose interpolation results includes:
  • multiple pose points are selected from the raw pose data, and a radar point corresponding to each pose point is selected from the raw point cloud data.
  • each pose point and the corresponding radar point are respectively substituted into the transformation point coordinate expression to obtain multiple equations to be solved with the external parameter matrix to be solved as the unknown quantity, and the multiple equations to be solved are combined to calculate the external parameter initial value.
  • step 250 in Figure 2 based on six known radar points ⁇ p 0 , p 1 , p 2 , p 3 , p 4 , p 5 ⁇ , six known pose points ⁇ q 0 , q 1 , q 2 , q 3 , q 4 , q 5 ⁇ and the coordinate expressions of the transformation points above, can construct six equations with the external parameter matrix to be obtained as the unknown quantity, and the six equations in the external parameter matrix to be obtained can be solved simultaneously unknowns, so as to obtain the solution of the external parameter matrix to be obtained, and this solution will be used as the initial T 0 of the external parameter.
  • Step 150 Based on the initial value of the external parameters and the pre-built optimization objective function, solve to obtain the optimal external parameter calibration solution.
  • the optimization objective function is constructed with the minimum sum of the distances of the closest points in the original point cloud data as the optimization objective, and then the optimal external parameter calibration solution is obtained by solving.
  • the initial value of the external parameter is gradually optimized through nonlinear optimization, and finally the optimal external parameter calibration solution is obtained.
  • the process of constructing an optimized objective function and solving the optimal external parameter calibration solution can be implemented in the following two ways:
  • the first way You can obtain the closest distance points of each radar point in the current frame in the original point cloud data in the next frame, construct multiple closest distance point pairs, and then calculate the distance of each closest distance point pair, and finally sum The sum of the distances of the nearest distance points can be obtained.
  • this method of solving the optimal external parameter calibration solution specifically includes:
  • the external parameter matrix to be found after the current assignment is used as the optimal external parameter calibration solution.
  • x i , y i , zi represent the coordinates of the i-th point in the point set ⁇ P under the coordinate system at the initial time t 0 of the integrated navigation device from the radar point
  • x′ i , y′ i , z′ i is the coordinate of the nearest point in the point set ⁇ P.
  • n the number of points in the point set.
  • the second method You can obtain the transformation points corresponding to each radar point in the original point cloud data, construct a transformation point set, and then obtain the two points closest to each point in the transformation point set, and obtain the distance between each point in the set. It is the distance between the two closest points (maybe including the current point itself), and finally sum the distances from each point to the two closest points to get the sum of the distances of the closest points.
  • the timestamp after time synchronization and the transformation point coordinate expression transform each radar point in each frame of the original point cloud data collected by the lidar to the combination of the initial time t 0
  • the transformation point corresponding to each radar point in the original point cloud data in the coordinate system at the initial moment of the integrated navigation device is obtained, and a set of transformation points is obtained.
  • each transformation Among the two adjacent transformation points corresponding to a point one of them may be the point itself, and the other is the actual closest distance point.
  • the distance between the two points A distance of zero does not affect the solution of the shortest distance.
  • the optimization objective function is constructed and optimized through continuous assignment iterations; until the sum of the distances of the closest distance points is less than the preset distance threshold or the number of assignment iterations reaches the preset threshold,
  • the current assigned extrinsic parameter matrix is used as the optimal extrinsic parameter calibration solution.
  • voxel filtering and Gaussian statistical filtering can be used.
  • voxel filtering processing can be performed first, and point cloud data can be down-sampled, and then filter out distance values greater than the preset distance threshold point, and then use the Statistical Outlier Removal algorithm (that is, the statistical outlier removal algorithm) to remove some outliers.
  • the Statistical Outlier Removal algorithm that is, the statistical outlier removal algorithm
  • the distance between the nearest 50 adjacent points of each point can be investigated. If the average distance is more than one standard deviation, the point is marked as an outlier and removed. After filtering the original point cloud data, it can greatly reduce the calculation amount of subsequent closest distance point pairs and improve the calculation efficiency.
  • the external parameter calibration device of the integrated navigation device and the laser radar provided by the application is described below.
  • the external parameter calibration device of the integrated navigation device and the laser radar described below and the external parameter calibration method of the integrated navigation device and the laser radar described above can be refer to each other.
  • Fig. 3 shows the external parameter calibration device of the combined navigation device and laser radar provided by the embodiment of the present application, the device includes:
  • the acquisition module 310 is used to acquire the original pose data collected by the integrated navigation device and the original point cloud data collected by the laser radar;
  • the first processing module 320 is used to time-synchronize the original point cloud data and the original pose data to obtain time-synchronized time stamp information;
  • the second processing module 330 is configured to perform pose interpolation on the original pose data according to the time-synchronized time stamp information and the original point cloud data, to obtain a pose interpolation result;
  • the third processing module 340 is used to calculate the initial value of the external parameter according to the original pose data, the original point cloud data and the pose interpolation result;
  • the fourth processing module 350 is configured to obtain an optimal external parameter calibration solution based on the initial value of the external parameter and the pre-built optimization objective function.
  • the second processing module 330 is specifically used for:
  • the adjacent pose points adjacent to the timestamp corresponding to each radar point in the original point cloud data are obtained from the original pose data;
  • the third processing module 340 is specifically used for:
  • transformation point coordinate expression is used to describe that the transformation point coordinates are obtained by multiplying the pose transformation matrix from the virtual point to the initial moment point of the integrated navigation device, the matrix of external parameters to be obtained, and the coordinates of the radar point.
  • the aforementioned external parameter matrix to be obtained may include a rotation angle and a translation amount.
  • the fourth processing module 350 is specifically configured to construct an optimization objective function based on the initial value of the external parameters, taking the minimum sum of the distances of the closest points in the original point cloud data as the optimization goal, and obtain the optimal external parameter calibration by solving untie.
  • fourth processing module 350 is specifically used for:
  • the external parameter matrix to be obtained after the current assignment is used as the optimal external parameter calibration solution.
  • fourth processing module 350 is specifically used for:
  • the external parameter matrix to be obtained after the current assignment is used as the optimal external parameter calibration solution.
  • the above-mentioned external parameter calibration device for the integrated navigation device and the laser radar may further include a filtering module, which is specifically configured to perform filtering processing on the original point cloud data.
  • the external parameter calibration device of the integrated navigation device and laser radar provided by the embodiment of the present application calculates a relatively ideal initial value of the external parameter through the third processing module, which can effectively improve the calibration efficiency and calibration accuracy; at the same time, The device uses the second processing module to directly estimate the pose of the lidar by interpolation method, which improves the calibration efficiency and reduces the computational complexity; in addition, the filter module filters the point cloud data collected by the lidar, which effectively improves the calibration efficiency. Efficiency, reducing the cost of calibration time.
  • the embodiment of the present application also provides an operation machine, which can realize high-precision positioning and perception functions in the operation process by using any of the above-mentioned external parameter calibration methods of the combined navigation device and laser radar.
  • the operating machine mentioned in this embodiment may be an excavator, and of course it may also be other operating machines that need to be equipped with high-precision positioning and sensing equipment.
  • an external parameter calibration system for a combined navigation device and laser radar the system includes:
  • the laser radar to be calibrated is used to collect the original point cloud data of the target
  • the integrated navigation device to be calibrated is used to collect the original pose data of the target
  • the controller is used to obtain the original pose data and the original point cloud data respectively; the original point cloud data and the original pose data are time-synchronized to obtain time-synchronized time stamp information; according to the time-synchronized time stamp information and the original Point cloud data, perform pose interpolation on the original pose data to obtain the pose interpolation result; calculate the initial value of the external parameter based on the original pose data, the original point cloud data and the pose interpolation result; and based on the initial value of the external parameter and The pre-built optimization objective function is solved to obtain the optimal external parameter calibration solution.
  • the hardware structure of the calibration system built in this embodiment is shown in Figure 4.
  • the hardware includes excavator power supply, controller (the model can be Nuvo-7160GC), RTK/IMU integrated navigation device (the model can be CGI-610), 3D Lidar (model can be VLP-32C), voltage regulator, voltage reducer, mouse and keyboard, display screen, etc.
  • the software part of the calibration system includes Ubuntu18.04 and ROS melodic operating systems, underlying driver software, and c++ programming language software.
  • the body of the RTK integrated navigation device CGI-610 and the controller Nuvo-7160GC are installed behind the seat in the cab of the excavator, and the RTK antenna of the integrated navigation device is installed at the rear of the upper body of the excavator , the distance between the two RTK antennas can be set as large as possible to obtain data with a slightly better signal; install the laser radar VLP-32C on the top of the cab or install it on the top of the cab through a bracket; put the mouse , keyboard and display are installed in front of the seat in the cab through the installation bracket.
  • the power supply of the excavator outputs 24V regulated voltage through the voltage regulator to supply power to the voltage reducer, controller Nuvo-7160GC, mouse and keyboard; the voltage reducer outputs 12V voltage to the RTK integrated navigation equipment CGI-610, Radar VLP-32C and display power supply.
  • the excavator bucket can be controlled to support the ground to perform pitch and rollover actions, so that the pitch angle, roll angle, and heading angle have large angle changes.
  • each point The distance of the nearest 50 adjacent points is inspected. If the distance of a point exceeds the average distance by more than one standard deviation, the point is marked as an outlier and removed. After filtering, the calculation amount of subsequent closest distance point pairs can be greatly reduced, and the calculation efficiency can be improved.
  • TOH time consists of two parts, one part is the number of minutes + seconds from the TOH time, the other part is the number of microseconds, and the lidar cannot express the time of more than an hour.
  • the NMEA GPGGA/GPRMC sentence in the RTK integrated navigation device CGI-610 provides the minute part and second part of UTC time, so the time stamp can be synchronized by reading this part of information.
  • For laser radar use the RTK/IMU integrated navigation pps time alignment function to obtain accurate time stamps. From the hardware level, it is necessary to connect the laser radar VLP-32C to the pps signal of the RTK integrated navigation device CGI-610 through the serial port, and consider the laser
  • the underlying driver of radar VLP-32C and the time stamp calculation mechanism of lidar VLP-32C point cloud points are used to calculate the time stamp of a single radar point.
  • the pose interpolation operation is performed, and the coordinate system at the initial time t0 of the RTK integrated navigation device CGI-610 is recorded as the world coordinate system, and the current time t k to the initial time t 0 of each pose point of the RTK integrated navigation device CGI-610 is calculated
  • the pose transformation matrix T_t 0 _t k is calculated
  • the two latest time stamps to t p are t k and t k+1 , and if t k ⁇ t p ⁇ t k+1 is satisfied, then t k in RTK integrated navigation equipment CGI-610
  • the transformation from time to initial time t 0 is T_t 0 _t k
  • the transformation from time t k+1 to initial time t 0 is T_t 0 _t k+1 ;
  • the pose transformation matrix T_t 0 _t p from the virtual point to the initial moment point of the integrated navigation device can be obtained to realize pose interpolation.
  • each radar point p i of the lidar is in the RTK/IMU integrated navigation device
  • the initial value of the external parameter calculates the initial value of the external parameter, and after completing the time synchronization and pose interpolation, calculate a relatively ideal initial value of the external parameter.
  • the initial value of the external parameters can effectively improve the efficiency of subsequent external parameter optimization and obtain a more accurate external parameter solution.
  • the five distances d1, d2, d3, d4, and d5 in this embodiment can be set to 5 meters, 10 meters, 15 meters, 20 meters, and 25 meters, respectively.
  • the calculation formula of pose point q i is shown in the following formula:
  • T 0 is the initial value of the external parameter, Indicates the pose transformation matrix of the lidar point p i in the coordinate system of the integrated navigation device at the initial moment;
  • T p i represents the virtual point coordinates of the laser radar point p i converted to the coordinate system of the integrated navigation device at the current moment;
  • T_t 0 _t p ⁇ T ⁇ p i represents the pose point coordinates of converting the lidar point p i into the coordinate system at the initial moment of the integrated navigation device.
  • the initial value T 0 ( ⁇ 0 , ⁇ 0 , ⁇ 0 , ⁇ 0 , b 01 , b 02 , b 03 ) of the external parameters can be obtained by solving the 6 unknowns with 6 equations.
  • the optimization objective function model is constructed, and solved through iterative optimization to obtain the optimal external parameter calibration solution T.
  • the optimal external parameter calibration solution T obtained in this embodiment can make the point cloud data of the lidar transformed into the coordinate system (ie, the world coordinate system) at the initial time t0 of the RTK/IMU integrated navigation device with the highest degree of self-coincidence.
  • FIG. 6 illustrates a schematic diagram of the physical structure of an electronic device.
  • the electronic device may include: a processor (processor) 610, a communication interface (Communications Interface) 620, a memory (memory) 630 and a communication bus 640, Wherein, the processor 610 , the communication interface 620 , and the memory 630 communicate with each other through the communication bus 640 .
  • the processor 610 can call the logic instructions in the memory 630 to execute the method for calibrating the external parameters of the integrated navigation device and the laser radar.
  • the method includes: respectively acquiring the original pose data collected by the integrated navigation device and the original point cloud data collected by the laser radar.
  • the logic instructions in the above-mentioned memory 630 may be implemented in the form of software functional units and when sold or used as an independent product, may be stored in a computer-readable storage medium.
  • the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments of the present application.
  • the aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disc, etc., which can store program codes. .
  • the present application also provides a computer program product
  • the computer program product includes a computer program stored on a non-transitory computer-readable storage medium
  • the computer program includes program instructions, and when the program instructions are executed by a computer During execution, the computer can execute the external parameter calibration method of the integrated navigation device and the laser radar provided by the above methods.
  • Point cloud data time-synchronize the original point cloud data with the original pose data to obtain the timestamp information after time synchronization; perform pose interpolation on the original pose data according to the time-synchronized timestamp information and the original point cloud data , to obtain the pose interpolation result; according to the original pose data, the original point cloud data and the pose interpolation result, calculate the initial value of the external parameter; based on the initial value of the external parameter and the pre-built optimization objective function, solve the optimal external parameter calibration untie.
  • the present application also provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, it is implemented to execute the external parameters of the above-mentioned integrated navigation device and laser radar.
  • a calibration method includes: for the same target, separately obtain the original pose data collected by the integrated navigation device and the original point cloud data collected by the laser radar; time-synchronize the original point cloud data and the original pose data to obtain time synchronization According to the timestamp information after time synchronization and the original point cloud data, perform pose interpolation on the original pose data to obtain the pose interpolation result; according to the original pose data, original point cloud data and pose interpolation As a result, the initial value of the external parameter is calculated; based on the initial value of the external parameter and the pre-built optimization objective function, the optimal external parameter calibration solution is obtained.
  • the device embodiments described above are only illustrative, and the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in One place, or it can be distributed to multiple network elements. Part or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. It can be understood and implemented by those skilled in the art without any creative effort.
  • each embodiment can be implemented by means of software plus a necessary general-purpose hardware platform, and of course also by hardware.
  • the essence of the above technical solution or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in computer-readable storage media, such as ROM/RAM, magnetic discs, optical discs, etc., including several instructions to make a computer device (which may be a personal computer, server, or network device, etc.) execute the methods described in various embodiments or some parts of the embodiments.

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请提供一种组合导航设备与激光雷达的外参标定方法及装置，本申请提供的组合导航设备与激光雷达的外参标定方法及装置，通过获取较为理想的外参初值，可以有效提高最优外参标定解的求取效率和结果精度，通过位姿插值的方式估计激光雷达的位姿信息，相比于通过跑SLAM算法进行雷达位姿估计的方式更加便捷、高效，进而降低了整个外参标定过程的耗时，使得外参标定过程更加高效，外参标定结果更加准确。

Description

组合导航设备与激光雷达的外参标定方法及装置
相关申请的交叉引用
本申请要求于2022年01月05日提交的申请号为202210006512.3，发明名称为“组合导航设备与激光雷达的外参标定方法及装置”的中国专利申请的优先权，其通过引用方式全部并入本文。
技术领域
本申请涉及智能驾驶技术领域，尤其涉及一种组合导航设备与激光雷达的外参标定方法及装置。
背景技术
高精度的组合导航设备和激光雷达是作业机械高精度定位与感知技术中常用的传感器，在实际应用过程中需要获得组合导航设备与激光雷达的位姿转换关系，但仅根据这两种传感器在作业机械上的安装位置，采用外参测量手段是很难获得外参标定参数的。
目前，组合导航设备与激光雷达的外参标定，主要通过将激光雷达采集的数据转换到组合导航设备的坐标系下实现。一方面，可以利用外界传感器直接进行外参测量，但由于组合导航设备与激光雷达的安装位置不同，直接测量过程操作复杂，且测量误差较大。
另一方面，还可以利用激光雷达跑SLAM算法的方式获得激光雷达的里程信息，再结合组合导航设备自身计算的里程信息，进行两坐标系间的变换矩阵计算，即可得到两传感器间的外参标定结果，然而，上述方法需要进行大量的计算，耗时长，且外参标定结果准确度较低。
发明内容
本申请提供一种组合导航设备与激光雷达的外参标定方法及装置，用以解决现有技术中组合导航设备与激光雷达的外参标定方法操作繁琐、耗时长且结果准确度低的缺陷，实现高效、准确的组合导航设备与激光雷达的外参标定。
第一方面，本申请提供一种组合导航设备与激光雷达的外参标定方法， 该方法包括：
分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；
将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；
根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；
根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；
基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果，包括：
分别获取所述原始位姿数据中每个位姿点当前时刻到初始时刻的位姿变换矩阵以及相应的时间戳信息，并获取所述每个雷达点在组合导航设备相应时刻坐标系下对应的虚拟点；
根据所述时间同步后的时间戳信息，从所述原始位姿数据中获取与所述原始点云数据中每个雷达点对应的时间戳相邻的相邻位姿点；
基于所述相邻位姿点的时间戳信息以及所述相应的位姿变换矩阵，计算所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵，作为位姿插值结果。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值，包括：
从所述原始位姿数据中选取多个位姿点，并从所述原始点云数据中选取与每个所述位姿点对应的雷达点；
分别将各个位姿点与相应的雷达点代入预先构建的变换点坐标表达式，获得多个以待求外参矩阵为未知量的待求解方程，将多个所述待求解方程联立，计算得到外参初值。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，所 述变换点坐标表达式用于描述变换点坐标是通过所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵、预先构建的待求外参矩阵以及雷达点坐标计算得到的。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解，包括：
基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：
基于所述外参初值，对待求外参矩阵进行赋值；
根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合；
分别在所述变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；
将各个所述变换点与其对应的至少两个邻近变换点的距离求和，得到最近距离点的距离之和；
以所述最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
直至所述最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
根据本申请提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：
基于所述外参初值，对待求外参矩阵进行赋值；
根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；
确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并 获取所述距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；
将所述第一变换点和所述第二变换点作为最近距离点对，并将所述原始点云数据中所有最近距离点对组合为最近距离点对集合；
计算所述最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；
以所述最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
直至所述最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
第二方面，一种组合导航设备与激光雷达的外参标定装置，该装置包括：
获取模块，用于分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；
第一处理模块，用于将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；
第二处理模块，用于根据所述时间同步后的时间戳信息以及原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；
第三处理模块，用于根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；
第四处理模块，用于基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
第三方面，本申请还提供一种作业机械，该作业机械使用上述任一种所述的组合导航设备与激光雷达的外参标定方法。
第四方面，本申请还提供一种组合导航设备与激光雷达的外参标定系统，该系统包括：
待标定激光雷达，用于采集目标物的原始点云数据；
待标定组合导航设备，用于采集目标物的原始位姿数据；
控制器，用于分别获取所述原始位姿数据以及所述原始点云数据；将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据所述时间同步后的时间戳信息以及原始点云数据，对所 述原始位姿数据进行位姿插值，得到位姿插值结果；根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；并基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
本申请提供的组合导航设备与激光雷达的外参标定方法及装置，通过获取较为理想的外参初值，可以有效提高最优外参标定解的求取效率和结果精度，通过位姿插值的方式估计激光雷达的位姿信息，相比于通过跑SLAM算法进行雷达位姿估计的方式更加便捷、高效，进而降低了整个外参标定过程的耗时，使得外参标定过程更加高效，外参标定结果更加准确。
附图说明
为了更清楚地说明本申请或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请提供的组合导航设备与激光雷达的外参标定方法的流程示意图；
图2是外参初值的计算流程示意图；
图3是本申请提供的组合导航设备与激光雷达的外参标定装置的结构示意图；
图4是实施过程中搭建的外参标定系统的硬件架构示意图；
图5是外参标定系统的工作流程示意图；
图6是本申请提供的电子设备的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请中的附图，对本申请中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
图1示出了本申请实施例提供的组合导航设备与激光雷达的外参标定方法，该方法包括：
步骤110：分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据。
本实施例中组合导航设备主要针对的是RTK/IMU组合导航设备，RTK(Real Time Kinematic，实时动态定位)，是一种卫星导航技术，用于提高从基于卫星的定位系统获得的位置数据的精度，除了信号所包含的信息之外，RTK还使用载波信号相位的测量值，并依靠单个参考站或内插虚拟站进行实时校正，以提供高达厘米级的定位精度。
IMU(Inertial Measurement Unit，惯性测量单元)，是主要用来检测和测量加速度与旋转运动的传感器。
因此，RTK/IMU组合导航设备可以同时获取卫星导航数据以及惯性测量数据，为后续外参标定过程提供可靠的数据支持。
本实施例中激光雷达主要针对的是3D激光雷达，3D激光雷达可以获取三维深度信息和点云数据，具有高分辨率和低功耗的优点，是作业机械上使用较为普遍的感知设备。
步骤120：将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息。
在时间同步环节，先将原始点云数据与原始位姿数据进行时间对齐，实现激光雷达与组合导航设备传输延迟的时间差补偿，本实施例设需要补偿的时间差为Δt 1。
然后基于需要补偿的时间差Δt 1、激光雷达初始时刻与组合导航设备初始时刻的时间差Δt 2、每个扫描帧的时间戳t h、每个扫描帧中每个点到该点所在的扫描帧开始时刻的时间戳t of，最终计算出每个扫描帧中每个点相对于组合导航设备初始时刻的时间戳t p，计算公式如下：
t p＝Δt 1+Δt 2+t h+t of        (1)
不难看出，本实施例中上述时间同步的过程，主要计算更新的是激光雷达的时间戳，目的是将激光雷达的时间戳尽量与组合导航设备的时间戳同步，以保证后续数据处理过程有效进行。
步骤130：根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果。
在示例性实施例中，根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果的过程，具体包括：
第一步：分别获取原始位姿数据中每个位姿点当前时刻到初始时刻的位姿变换矩阵以及相应的时间戳信息，并获取每个雷达点在组合导航设备相应时刻坐标系下对应的虚拟点。
本实施例具体通过对组合导航设备的位姿点做积分变换，通过IMU积分变换，计算每个位姿点当前时间t k时刻到初始时刻t 0的位姿变换矩阵T_t 0_t k。雷达点在组合导航设备相应时刻坐标系下对应的虚拟点，可以通过雷达点的坐标乘以预设的待求外参矩阵获得。
第二步：根据时间同步后的时间戳信息，从原始位姿数据中获取与原始点云数据中每个雷达点对应的时间戳相邻的相邻位姿点。
假设时间同步后激光雷达采集原始点云数据中某雷达点p i的时间戳为t p，在组合导航设备中与时间戳t p最近的两个时间戳为t k和t k+1且满足t k≤t p＜t k+1，相应的可以获知t k和t k+1时刻对应的位姿点，即相邻位姿点，同时，能够获知t k时刻对应位姿点当前时刻到初始时刻的位姿变换矩阵T_t 0_t k，以及t k+1时刻对应位姿点当前时刻到初始时刻的位姿变换矩阵T_t 0_t k+1。
第三步：基于相邻位姿点的时间戳信息以及相应的位姿变换矩阵，计算虚拟点到组合导航设备初始时刻点的位姿变换矩阵，作为位姿插值结果。
以上述第二步假设内容为例，该步中虚拟点到组合导航设备初始时刻点的位姿变换矩阵，即虚拟位姿变换矩阵T_t 0_t p的计算公式如下：
Figure PCTCN2023070132-appb-000001
可以理解的是，以上述假设内容为例，本实施例提到的虚拟点，指的是组合导航坐标系下，t p时刻对应的点，由于该点在组合导航坐标系下实际并不存在，因此将该点称为虚拟点。
步骤140：根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值。
为了实现组合导航设备与激光雷达的外参标定，本实施例预先构建了一个待求外参矩阵，该待求外参矩阵中包含旋转角度和平移量。
该待求外参矩阵具体可以包含三个旋转角度和三个平移量，比如旋转角度可以是航向角α、横滚角β和偏航角γ，平移量可以是X坐标轴上的平移量b 1、Y坐标轴上的平移量b 2以及Z坐标轴上的平移量b 3，这样，待 求外参矩阵T可以表示为：
Figure PCTCN2023070132-appb-000002
其中，R表示旋转矩阵，t表示平移矩阵，a 1至a 9表示旋转矩阵中的元素，b 1至b 3表示平移矩阵中的元素。
因此，该待求外参矩阵T包含六个未知数，通过求解上述六个未知数可以得到该待求外参矩阵的求解结果，即可获得外参标定解。
这样，基于上述位姿插值结果、预先构建的待求外参矩阵以及雷达点坐标，可以构建变换点坐标表达式，该变换点坐标表达式用于描述变换点坐标是通过虚拟点到组合导航设备初始时刻点的位姿变换矩阵、待求外参矩阵以及雷达点坐标的乘积得到的。
可以理解的是，由于每个雷达点到组合导航设备初始时刻坐标系的位姿变换矩阵可以由待求解的外参矩阵左乘对应的虚拟位姿变换矩阵得到，因此，在计算每个雷达点在组合导航设备初始时刻坐标系下对应的变换点的坐标时，可以由雷达点坐标左乘其到组合导航设备初始时刻坐标系的位姿变换矩阵得到。
具体地，以上述雷达点p i在组合导航设备初始时刻坐标系下的变换点q i的坐标表达式为例进行说明，即：
Figure PCTCN2023070132-appb-000003
其中，
Figure PCTCN2023070132-appb-000004
表示激光雷达点p i在组合导航设备初始时刻坐标系下的位姿变换矩阵；T·p i表示将激光雷达点p i转换到当前时刻组合导航设备坐标系下的虚拟点坐标；T_t 0_t p·T·p i表示将激光雷达点p i转换到组合导航设备初始时刻坐标系下的变换点坐标。
参见附图2，根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值的过程，具体包括：
首先，从原始位姿数据中选取多个位姿点，并从原始点云数据中选取与每个位姿点对应的雷达点。
本实施例在完成时间同步和位姿插值后，一方面，从组合导航设备采集的原始位姿数据中提取空间位置距离分别大于d 1、d 2、d 3、d 4、d 5的五个 位姿点q 1、q 2、q 3、q 4、q 5，基于已知的初始位姿点q 0，可以得到六个已知的位姿点，即图2所示的步骤210和220；
另一方面，在激光雷达采集的原始点云数据中提取与五个位姿点q 1、q 2、q 3、q 4、q 5对应的五个雷达点p 1、p 2、p 3、p 4、p 5，基于已知的初始雷达点p 0，可以得到六个已知的雷达点，即图2所示的步骤230和240。
然后，分别将各个位姿点与相应的雷达点代入变换点坐标表达式，获得多个以待求外参矩阵为未知量的待求解方程，将多个待求解方程联立，计算得到外参初值。
参见附图2中步骤250，基于六个已知雷达点{p 0，p 1，p 2，p 3，p 4，p 5}、六个已知位姿点{q 0，q 1，q 2，q 3，q 4，q 5}以及上述变换点坐标表达式，可以构建六个以待求外参矩阵为未知量的方程，联立六个方程可以求解待求外参矩阵内的六个未知数，从而得到待求外参矩阵的解，将该解将作为外参初始T 0。
步骤150：基于外参初值进行以及预先构建的优化目标函数，求解得到最优外参标定解。
具体地，本实施例通过基于外参初值，以原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，进而求解得到最优外参标定解。
为了获得最优的外参标定解，本实施例通过非线性优化的方式以外参初值为初始值逐步寻优，最终获得最优外参标定解。
在示例性实施例中，构建优化目标函数并求解最优外参标定解的过程，可以通过如下两种方式实现：
第一种方式：可以获取原始点云数据中当前帧内各个雷达点在下一帧内的最近距离点，构建多个最近距离点对，进而求取每个最近距离点对的距离，最后求和即可得到最近距离点的距离之和。
具体地，该种求解最优外参标定解的方式，具体包括：
首先，基于外参初值，对待求外参矩阵进行赋值；
然后，根据赋值后的待求外参矩阵和变换点坐标表达式，获取原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；
确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并 获取距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；
接着，将第一变换点和第二变换点作为最近距离点对，并将原始点云数据中所有最近距离点对组合为最近距离点对集合；
之后，计算最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；
再然后，以最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
最后，直至最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
该种方式中，需要计算激光雷达采集的原始点云数据中每个扫描帧内每个雷达点与其对应的下一扫描帧内距离最近的雷达点，并利用预先赋值的待求外参矩阵以及变换点坐标表达式，分别获取每个雷达点与其最近距离点在组合导航设备初始时刻坐标系下的变换点，将当前雷达点对应的变换点与其最近距离点对应的变换点记为最近距离点对，将原始点云数据中所有的最近距离点对组合，构建最近距离点对集合。
后续计算最近距离点对集合中所有最近距离点对的距离之和，可以得到原始点云数据中最近距离点的距离之和，具体可以表示为：
Figure PCTCN2023070132-appb-000005
其中，x i，y i，z i表示雷达点转到组合导航设备初始时刻t 0时刻坐标系下的点集合∑P中的第i个点的坐标，x′ i，y′ i，z′ i为点集合∑P中距离该点最近的点坐标。
以最近距离点的距离之和D error最小为优化目标，可以构建如下优化目标函数，即：
Figure PCTCN2023070132-appb-000006
其中，n表示点集合中点的数量。
利用非线性优化算法进行不断迭代与优化，直到某次迭代后的D error小于设定的距离阈值D th或在当前迭代次数大于最大迭代次数(即预设次数阈值)时，停止迭代，此时的T就是最优的外参标定解。
第二种方式：可以求取原始点云数据中各雷达点对应的变换点，构建 变换点集合，然后求取变换点集合中各点距离最近的两个点，并求取集合中各点到其距离最近的两个点(可能包含当前点本身)的距离，最后将各个点到距离最近的两个点的距离求和，即可得到最近距离点的距离之和。
该种方式在实际应用过程中，具体流程如下：
首先，利用外参初值T 0，对待求外参矩阵进行赋值；
然后，根据赋值后的待求外参矩阵、时间同步后的时间戳以及变换点坐标表达式，将激光雷达采集的原始点云数据中每帧内每个雷达点变换到t 0初始时刻的组合导航设备坐标系下，即获取原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合。
接着，分别在变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；本实施例中每个变换点对应的两个邻近变换点中，其中一个可能为该点本身，另一个为实际的最近距离点，在距离的求取过程中，如果其中一个邻近变换点为该点本身，则两点间距离为零，并不影响最近距离的求解。
之后，将各个变换点与其对应的至少两个邻近变换点的距离求和，得到最近距离点的距离之和；
最后，以最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；直至最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
上述两种方式中，可以利用现有的邻近搜索算法来求取最近距离点，比如K-D树(K-Dimensional Tree的简称)。
为了进一步提高数据处理效率和准确性，在将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息之前，还可以包括：
对原始点云数据进行滤波处理。
本实施例中可以采用体素滤波和高斯统计滤波，具体地，在进行滤波处理时，可以先进行体素滤波处理，对点云数据进行降采样，然后滤去距离值大于预设距离阈值以外的点，再利用Statistical Outlier Removal算法(即统计学异常点移除算法)移除部分离群点，比如，可以对每个点最近的50个相邻点距离进行考察，如果一个点的距离超出平均距离一个标准差以上，则该点被标记为离群点并移除。对原始点云数据滤波后可大大减 少后续最近距离点对的计算量，提高计算效率。
下面对本申请提供的组合导航设备与激光雷达的外参标定装置进行描述，下文描述的组合导航设备与激光雷达的外参标定装置与上文描述的组合导航设备与激光雷达的外参标定方法可相互对应参照。
图3示出了本申请实施例提供的组合导航设备与激光雷达的外参标定装置，该装置包括：
获取模块310，用于分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；
第一处理模块320，用于将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；
第二处理模块330，用于根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；
第三处理模块340，用于根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；
第四处理模块350，用于基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
在示例性实施例中，第二处理模块330，具体用于：
分别获取原始位姿数据中每个位姿点当前时刻到初始时刻的位姿变换矩阵以及相应的时间戳信息，并获取每个雷达点在组合导航设备相应时刻坐标系下对应的虚拟点；
根据时间同步后的时间戳信息，从原始位姿数据中获取与原始点云数据中每个雷达点对应的时间戳相邻的相邻位姿点；
基于相邻位姿点的时间戳信息以及相应的位姿变换矩阵，计算虚拟点到组合导航设备初始时刻点的位姿变换矩阵，作为位姿插值结果。
在示例性实施例中，第三处理模块340，具体用于：
从原始位姿数据中选取多个位姿点，并从原始点云数据中选取与每个位姿点对应的雷达点；
分别将各个位姿点与相应的雷达点代入预先构建的变换点坐标表达式，获得多个以待求外参矩阵为未知量的待求解方程，将多个待求解方程联立，计算得到外参初值。
需要说明的是，变换点坐标表达式用于描述变换点坐标是通过虚拟点 到组合导航设备初始时刻点的位姿变换矩阵、待求外参矩阵以及雷达点坐标的乘积得到的。
需要说明的是，上述提到的待求外参矩阵可以包括旋转角度和平移量。
在示例性实施例中，第四处理模块350具体用于基于外参初值，以原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解。
进一步地，上述第四处理模块350具体用于：
基于外参初值，对待求外参矩阵进行赋值；
根据赋值后的待求外参矩阵和变换点坐标表达式，获取原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合；
分别在变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；
将各个变换点与其对应的至少两个邻近变换点的距离求和，得到最近距离点的距离之和；
以最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
直至最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
进一步地，上述第四处理模块350具体用于：
基于外参初值，对待求外参矩阵进行赋值；
根据赋值后的待求外参矩阵和变换点坐标表达式，获取原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；
确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；
将第一变换点和第二变换点作为最近距离点对，并将原始点云数据中所有最近距离点对组合为最近距离点对集合；
计算最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；
以最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
直至最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
在示例性实施例中，上述组合导航设备与激光雷达的外参标定装置还可以包括滤波模块，该滤波模块具体用于对原始点云数据进行滤波处理。
由此可见，本申请实施例提供的组合导航设备与激光雷达的外参标定装置，通过第三处理模块计算出了一个较为理想的外参初值，能够有效提高标定效率与标定精度；同时，该装置通过第二处理模块直接用插值法估计激光雷达的位姿，提高了标定效率，降低了计算复杂量；此外，通过滤波模块对激光雷达采集的点云数据进行滤波处理，有效提高了标定效率，降低了标定时间成本。
另外，本申请实施例还提供一种作业机械，该作业机械使用上述任一种的组合导航设备与激光雷达的外参标定方法，可以实现作业过程的高精度定位和感知功能。
可以理解的是，本实施例提到的作业机械可以是挖掘机，当然也可以是其他需要安装高精度定位和感知设备的作业机械。
此外，本申请实施例还提供一种组合导航设备与激光雷达的外参标定系统，该系统包括：
待标定激光雷达，用于采集目标物的原始点云数据；
待标定组合导航设备，用于采集目标物的原始位姿数据；
控制器，用于分别获取原始位姿数据以及原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；并基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
下面以挖掘机为例，对组合导航设备与激光雷达的外参标定系统的实施方案进行详细说明。
本实施例搭建的标定系统的硬件结构架构如图4所示，硬件包括挖掘机电源、控制器(型号可以为Nuvo-7160GC)、RTK/IMU组合导航设备 (型号可以为CGI-610)、3D激光雷达(型号可以为VLP-32C)、稳压器、降压器、鼠标键盘、显示屏等。该标定系统的软件部分包括Ubuntu18.04以及ROS melodic的操作系统、底层驱动程序软件以及c++编程语言软件等。
标定系统硬件在安装时，将RTK组合导航设备CGI-610的机身和控制器Nuvo-7160GC安装于挖掘机驾驶室内座位的后方位置，将组合导航设备的RTK天线装于挖掘机上车体后方位置，两个RTK天线的距离可以设置的尽可能大些，以获取到信号轻度更优的数据；将激光雷达VLP-32C安装于驾驶室顶部或通过支架安装于驾驶室顶部上方位置；将鼠标、键盘和显示屏通过安装支架装于驾驶室内座位的前方。
如图4所示，挖掘机电源通过稳压器输出24V稳压，给降压器、控制器Nuvo-7160GC以及鼠标和键盘供电；降压器输出12V电压给RTK组合导航设备CGI-610、激光雷达VLP-32C以及显示屏供电。
硬件安装完毕后，启动挖掘机匀速行走，并将上述搭建的系统开启。操作挖掘机进行一些非平面运动以获取姿态上的变化数据，具体地，可操控挖掘机铲斗撑地进行俯仰、侧翻动作，使得俯仰角、翻滚角以及航向角有很大的角度变化。
然后将RTK组合导航设备CGI-610采集到的原始位姿数据和激光雷达VLP-32C采集的原始点云数据输入到控制器Nuvo-7160GC，利用标定技术实现RTK组合导航设备CGI-610和激光雷达VLP-32C的离线外参标定，外参标定过程的算法流程如图5所示。
参见附图5，首先，读取RTK组合导航设备CGI-610采集的原始数据，即CGI-610数据，并对其进行GPCHC数据协议解析，从而获取CGI-610的位置(即WGS84坐标)与姿态数据，包括时间戳、经纬高度(即经度、纬度和高度)以及三个姿态角(即俯仰角、翻滚角和航向角)，并将解析出来的WGS84坐标进行坐标转换，以获取t 0初始时刻CGI-610坐标系下的位姿数据。
然后，读取激光雷达VLP-32C采集的原始数据，即VLP-32C数据，并进行数据解析以获取pointcloud2格式的点云数据。之后对获得的点云数据进行滤波处理。滤波处理时，先进行体素滤波处理，对点云数据进行降采样；然后滤去距离值大于50m以外的点；再利用Statistical Outlier Removal算法移除一部分离群点，本实施例对每个点最近的50个相邻点距离进行 考察，如果一个点的距离超出平均距离一个标准差以上，则该点被标记为离群点并移除。滤波后可大大减少后续最近距离点对的计算量，提高计算效率。
接着，先通过时间对齐消除两传感器之间的延迟时间差，再对RTK组合导航设备CGI-610和激光雷达VLP-32C进行时间同步，由于激光雷达VLP-32C内部只有一个代表TOH(Top Of Hour)时间的计数器，TOH时间由两部分组成，一部分是从TOH时间开始的分钟数+秒数，另一部分是微秒数，且激光雷达不能表达整小时以上的时间。
而RTK组合导航设备CGI-610中的NMEA GPGGA/GPRMC语句提供了UTC时间的分钟部分和秒部分，因此可通过读取这部分信息来进行时间戳同步。对于激光雷达使用RTK/IMU组合导航pps时间对齐功能获取精确的时间戳，从硬件层面来说，需要通过串口为激光雷达VLP-32C接入RTK组合导航设备CGI-610的pps信号，并考虑激光雷达VLP-32C底层驱动和激光雷达VLP-32C点云点的时间戳计算机制来计算单个雷达点的时间戳。
之后，进行位姿插值操作，记RTK组合导航设备CGI-610初始时刻t0下的坐标系为世界坐标系，计算RTK组合导航设备CGI-610每个位姿点当前时刻t k到初始时刻t 0的位姿变换矩阵T_t 0_t k；
假设激光雷达每个点p i相对于RTK组合导航设备CGI-610初始时刻的时间戳为t p(即时间同步后的时间戳)；
RTK组合导航设备CGI-610中与t p最近的两个时间戳为t k、t k+1，且满足t k≤t p＜t k+1，则RTK组合导航设备CGI-610中t k时刻到初始时刻t 0的变换为T_t 0_t k，t k+1时刻到初始时刻t 0的变换为T_t 0_t k+1；
基于上述信息，可以得到虚拟点到组合导航设备初始时刻点的位姿变换矩阵T_t 0_t p，以实现位姿插值。
基于t p、t k、t k+1、T_t 0_t k、T_t 0_t k+1以及位姿插值结果T_t 0_t p，可以估计激光雷达每个雷达点p i在RTK/IMU组合导航设备初始时刻坐标系下的位姿点q i。即为每个扫描帧中每个雷达点通过计算的时间戳去匹配对应的更准确的位置和姿态值。
再然后，计算外参初值，完成时间同步与位姿插值后，计算出一个相对理想的外参初值。该外参初值能够有效提高后续外参优化的效率，并获 得精度更高的外参解。结合附图2，本实施例中5个距离d1、d2、d3、d4、d5可以分别设置为5米、10米、15米、20米、25米。位姿点q i的计算公式如下式所示：
Figure PCTCN2023070132-appb-000007
其中，i＝0，1，…，5，T 0为外参初值，
Figure PCTCN2023070132-appb-000008
表示激光雷达点p i在组合导航设备初始时刻坐标系下的位姿变换矩阵；T·p i表示将激光雷达点p i转换到当前时刻组合导航设备坐标系下的虚拟点坐标；T_t 0_t p·T·p i表示将激光雷达点p i转换到组合导航设备初始时刻坐标系下的位姿点坐标。
因此，用6个等式求解6个未知数，从而可以求出外参初值T 0(α 0，β 0，γ 0，b 01，b 02，b 03)。
最后，以最近距离点的距离之和最小为优化目标，构建优化目标函数模型，并通过迭代优化求解，得到最优外参标定解T。
本实施例求得的最优外参标定解T可以使得激光雷达的点云数据变换到RTK/IMU组合导航设备初始时刻t0时刻的坐标系(即世界坐标系)后自我重合度最高。
图6示例了一种电子设备的实体结构示意图，如图6所示，该电子设备可以包括：处理器(processor)610、通信接口(Communications Interface)620、存储器(memory)630和通信总线640，其中，处理器610，通信接口620，存储器630通过通信总线640完成相互间的通信。处理器610可以调用存储器630中的逻辑指令，以执行组合导航设备与激光雷达的外参标定方法，该方法包括：分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
此外，上述的存储器630中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算 机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本申请还提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法所提供的组合导航设备与激光雷达的外参标定方法，该方法包括：针对同一目标物，分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
又一方面，本申请还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各提供的组合导航设备与激光雷达的外参标定方法，该方法包括：针对同一目标物，分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通 过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (10)

  1. 一种组合导航设备与激光雷达的外参标定方法，包括：
    分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；
    将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；
    根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；
    根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；
    基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
  2. 根据权利要求1所述的一种组合导航设备与激光雷达的外参标定方法，其中，根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果，包括：
    分别获取所述原始位姿数据中每个位姿点当前时刻到初始时刻的位姿变换矩阵以及相应的时间戳信息，并获取所述每个雷达点在组合导航设备相应时刻坐标系下对应的虚拟点；
    根据所述时间同步后的时间戳信息，从所述原始位姿数据中获取与所述原始点云数据中每个雷达点对应的时间戳相邻的相邻位姿点；
    基于所述相邻位姿点的时间戳信息以及所述相应的位姿变换矩阵，计算所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵，作为位姿插值结果。
  3. 根据权利要求1所述的一种组合导航设备与激光雷达的外参标定方法，其中，根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值，包括：
    从所述原始位姿数据中选取多个位姿点，并从所述原始点云数据中选取与每个所述位姿点对应的雷达点；
    分别将各个位姿点与相应的雷达点代入预先构建的变换点坐标表达式，获得多个以待求外参矩阵为未知量的待求解方程，将多个所述待求解 方程联立，计算得到外参初值。
  4. 根据权利要求3所述的一种组合导航设备与激光雷达的外参标定方法，其中，所述变换点坐标表达式用于描述变换点坐标是通过所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵、预先构建的待求外参矩阵以及雷达点坐标计算得到的。
  5. 根据权利要求3所述的一种组合导航设备与激光雷达的外参标定方法，其中，基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解，包括：
    基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解。
  6. 根据权利要求5所述的一种组合导航设备与激光雷达的外参标定方法，其中，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：
    基于所述外参初值，对待求外参矩阵进行赋值；
    根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合；
    分别在所述变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；
    将各个所述变换点与其对应的至少两个邻近变换点的距离求和，得到最近距离点的距离之和；
    以所述最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
    直至所述最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
  7. 根据权利要求5所述的一种组合导航设备与激光雷达的外参标定方法，其中，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：
    基于所述外参初值，对待求外参矩阵进行赋值；
    根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；
    确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取所述距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；
    将所述第一变换点和所述第二变换点作为最近距离点对，并将所述原始点云数据中所有最近距离点对组合为最近距离点对集合；
    计算所述最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；
    以所述最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；
    直至所述最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。
  8. 一种组合导航设备与激光雷达的外参标定装置，包括：
    获取模块，用于分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；
    第一处理模块，用于将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；
    第二处理模块，用于根据所述时间同步后的时间戳信息以及原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；
    第三处理模块，用于根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；
    第四处理模块，用于基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
  9. 一种作业机械，该作业机械使用如权利要求1至7任一项所述的一种组合导航设备与激光雷达的外参标定方法。
  10. 一种组合导航设备与激光雷达的外参标定系统，包括：
    待标定激光雷达，用于采集目标物的原始点云数据；
    待标定组合导航设备，用于采集目标物的原始位姿数据；
    控制器，用于分别获取所述原始位姿数据以及所述原始点云数据；将 所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据所述时间同步后的时间戳信息以及原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；并基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。
PCT/CN2023/070132 2022-01-05 2023-01-03 组合导航设备与激光雷达的外参标定方法及装置 WO2023131123A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210006512.3A CN114325667A (zh) 2022-01-05 2022-01-05 组合导航设备与激光雷达的外参标定方法及装置
CN202210006512.3 2022-01-05

Publications (1)

Publication Number Publication Date
WO2023131123A1 true WO2023131123A1 (zh) 2023-07-13

Family

ID=81024937

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/070132 WO2023131123A1 (zh) 2022-01-05 2023-01-03 组合导航设备与激光雷达的外参标定方法及装置

Country Status (2)

Country Link
CN (1) CN114325667A (zh)
WO (1) WO2023131123A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115267751A (zh) * 2022-08-19 2022-11-01 广州小鹏自动驾驶科技有限公司 传感器标定方法、装置、车辆及存储介质
CN117036511A (zh) * 2023-10-08 2023-11-10 之江实验室 多类型传感器的标定方法、装置、计算机设备和存储介质
CN117150754A (zh) * 2023-08-28 2023-12-01 中国民用航空总局第二研究所 基于仿真的激光雷达外参获取方法、电子设备和存储介质
CN117706530A (zh) * 2024-02-05 2024-03-15 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统
CN117853594A (zh) * 2024-03-07 2024-04-09 武汉普惠海洋光电技术有限公司 海底阵潜标位置标定方法、装置、存储介质及电子设备

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114325667A (zh) * 2022-01-05 2022-04-12 上海三一重机股份有限公司 组合导航设备与激光雷达的外参标定方法及装置
CN115435816B (zh) * 2022-11-07 2023-04-11 山东大学 在线双舵轮agv内外参标定方法、系统、介质及设备
CN115979277B (zh) * 2023-02-22 2023-06-02 广州导远电子科技有限公司 时间同步方法、装置、电子设备和计算机可读存储介质
CN116449347B (zh) * 2023-06-14 2023-10-03 蘑菇车联信息科技有限公司 路侧激光雷达的标定方法、装置及电子设备
CN116908818B (zh) * 2023-07-13 2024-05-28 广东喜讯智能科技有限公司 基于rtk无人机的激光雷达的标定方法、装置及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180356824A1 (en) * 2017-06-13 2018-12-13 TuSimple Time synchronization and data acquisition method for ground truth static scene sparse flow generation
CN110244284A (zh) * 2019-07-29 2019-09-17 南通润邦重机有限公司 一种用于多线激光雷达和gps\ins标定的标定板及其方法
CN112285676A (zh) * 2020-10-22 2021-01-29 知行汽车科技（苏州）有限公司 激光雷达与imu外参标定方法及装置
CN113888649A (zh) * 2021-10-18 2022-01-04 上海振华重工(集团)股份有限公司 一种多激光雷达外参标定方法、装置、设备及存储介质
CN114325667A (zh) * 2022-01-05 2022-04-12 上海三一重机股份有限公司 组合导航设备与激光雷达的外参标定方法及装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180356824A1 (en) * 2017-06-13 2018-12-13 TuSimple Time synchronization and data acquisition method for ground truth static scene sparse flow generation
CN110244284A (zh) * 2019-07-29 2019-09-17 南通润邦重机有限公司 一种用于多线激光雷达和gps\ins标定的标定板及其方法
CN112285676A (zh) * 2020-10-22 2021-01-29 知行汽车科技（苏州）有限公司 激光雷达与imu外参标定方法及装置
CN113888649A (zh) * 2021-10-18 2022-01-04 上海振华重工(集团)股份有限公司 一种多激光雷达外参标定方法、装置、设备及存储介质
CN114325667A (zh) * 2022-01-05 2022-04-12 上海三一重机股份有限公司 组合导航设备与激光雷达的外参标定方法及装置

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115267751A (zh) * 2022-08-19 2022-11-01 广州小鹏自动驾驶科技有限公司 传感器标定方法、装置、车辆及存储介质
CN117150754A (zh) * 2023-08-28 2023-12-01 中国民用航空总局第二研究所 基于仿真的激光雷达外参获取方法、电子设备和存储介质
CN117150754B (zh) * 2023-08-28 2024-04-19 中国民用航空总局第二研究所 基于仿真的激光雷达外参获取方法、电子设备和存储介质
CN117036511A (zh) * 2023-10-08 2023-11-10 之江实验室 多类型传感器的标定方法、装置、计算机设备和存储介质
CN117036511B (zh) * 2023-10-08 2024-03-26 之江实验室 多类型传感器的标定方法、装置、计算机设备和存储介质
CN117706530A (zh) * 2024-02-05 2024-03-15 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统
CN117706530B (zh) * 2024-02-05 2024-05-14 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统
CN117853594A (zh) * 2024-03-07 2024-04-09 武汉普惠海洋光电技术有限公司 海底阵潜标位置标定方法、装置、存储介质及电子设备

Also Published As

Publication number Publication date
CN114325667A (zh) 2022-04-12

Similar Documents

Publication Publication Date Title
WO2023131123A1 (zh) 组合导航设备与激光雷达的外参标定方法及装置
JP6862409B2 (ja) 地図生成及び移動主体の位置決めの方法及び装置
CN112859051B (zh) 激光雷达点云运动畸变的矫正方法
CN1959343B (zh) 机载多角度成像测高技术
WO2022007776A1 (zh) 目标场景区域的车辆定位方法、装置、设备和存储介质
US20110218733A1 (en) Method and apparatus for vision aided navigation using image registration
US11619724B2 (en) Calibration of multiple lidars mounted on a vehicle using localization based on a high definition map
CN115407357B (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
RU2487419C1 (ru) Система комплексной обработки информации радионавигационных и автономных средств навигации для определения действительных значений параметров самолетовождения
CN112731358B (zh) 一种多激光雷达外参在线标定方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN112987065A (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN112086010A (zh) 地图生成方法、装置、设备及存储介质
CN114413887A (zh) 一种传感器外部参数标定方法、设备及介质
CN115330866A (zh) 一种融合激光雷达和深度相机点云的建图定位方法
CN112861660A (zh) 激光雷达阵列与相机同步装置、方法、设备及存储介质
CN114217665A (zh) 一种相机和激光雷达时间同步方法、装置及存储介质
CN111829514A (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
CN115526914A (zh) 基于多传感器的机器人实时定位和彩色地图融合映射方法
CN115728753A (zh) 激光雷达与组合导航的外参标定方法、装置及智能车辆
CN113959437A (zh) 一种用于移动测量设备的测量方法及系统
CN116203544A (zh) 一种移动测量系统往返测回无控自检校方法、装置及介质
WO2023226156A1 (zh) 时间戳校正方法、装置、设备、介质及计算机程序产品
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
WO2022256976A1 (zh) 稠密点云真值数据的构建方法、系统和电子设备

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23736988

Country of ref document: EP

Kind code of ref document: A1