Multi-sensor joint calibration method and system based on speed optimization

Abstract

The invention discloses a multi-sensor joint calibration method based on speed optimization, comprising the following steps: S1, m sensors collect data and timestamps during the driving process of the mobile robot, and calculate the speed of the sensor at the time of collection; S2, obtain the speed values ​​of the m sensors in the same time series by interpolation method, and put them into the data set Sensor_list; S3, convert the speed values ​​of different sensors in the base coordinate system into a least squares problem, and solve the least squares nonlinearly to obtain the transformation matrix of each sensor relative to the base coordinate system. The invention has no requirements on the type, performance and number of sensors to be calibrated, and has strong scalability; it is insensitive to the accumulated error of the sensor itself, and has high calibration accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and more specifically, the present invention relates to a multi-sensor joint calibration method and system based on speed optimization. Background Art

[0002] With the development of science and technology, mobile robots have been increasingly widely used in the fields of warehousing logistics and intelligent inspection. Environmental perception technology is an important part of mobile robot technology and the basis for mobile robots to perform tasks. Relying on a single sensor to perceive the environment often cannot achieve satisfactory results. Therefore, multiple sensors are installed on the body of the mobile robot to complement and optimize the information of various sensors, and finally produce the optimal observation results of the environment, such as IMU, GPS, camera and lidar. Before the comprehensive application of multiple sensors, it is necessary to know the relative posture transformation of each sensor, that is, the calibration of the sensor, and it is necessary to calibrate the transformation matrix of each sensor to the base coordinate system.

[0003] Existing calibration methods calculate the positioning results of different sensors to obtain the relative relationship between sensors, such as the calibration between LiDAR and odometer or IMU. However, the above calibration schemes have poor scalability and cannot adapt to changes in sensor types and quantities. In addition, the calibration results will be affected by the accumulated errors of the sensor's own measurements. Summary of the invention

[0004] The present invention provides a multi-sensor joint calibration method based on speed optimization, aiming to provide a multi-sensor calibration method with strong scalability and high calibration accuracy.

[0005] The present invention is implemented as follows: a multi-sensor joint calibration method based on speed optimization, the method specifically comprising the following steps:

[0006] S1, m sensors collect data and timestamps during the mobile robot's driving process, and calculate the speed of the sensor at the time of collection;

[0007] S2. Obtain the speed values ​​of m sensors in the same time series through interpolation method and put them into the data set Sensor_list;

[0008] S3. The speed values ​​of different sensors in the base coordinate system are converted into a least squares problem. The transformation matrix of each sensor relative to the base coordinate system can be obtained by nonlinearly solving the least squares problem.

[0009] Furthermore, the method for obtaining the speed values ​​of the m sensors in the same time series is specifically as follows:

[0010] S21, putting the timestamp of each sensor into the timestamp container Time_all, arranging them in order, and deleting redundant timestamps;

[0011] S22, sequentially select the data set S of each sensor in the data set Sensor_list, interpolate the data set S, and obtain the speed values ​​of all sensors in the same time series, where the data set S of sensor i i The interpolation process is as follows:

[0012] Traverse all timestamps t in the timestamp container Time_all, and check whether there is a timestamp with timestamp t in the data set S. If the detection result is no, calculate the speed value v of sensor i at timestamp t based on the interpolation method t , insert the data set S i middle.

[0013] Furthermore, in the dataset S i Find two timestamps t adjacent to time t i ,t i+1 , where t i <t<t i+1 , the velocity value v of sensor i at timestamp t t The calculation formula is as follows:

[0014]

[0015] in, They represent sensor i at timestamp t i ,t i+1 The speed value below.

[0016] Furthermore, the least squares problem is formulated as follows:

[0017]

[0018] By solving the least squares problem nonlinearly, the precise transformation matrix T_list = {T1, T2, ..., T m}.

[0019] The present invention is implemented as follows: a multi-sensor joint calibration system based on speed optimization, the system comprising:

[0020] Set m sensors on the same mobile robot and a processing unit connected to the m sensors. The processing unit calibrates the transformation matrix T_list = {T1, T2, ..., T m}.

[0021] The present invention has no requirements on the type, performance and quantity of sensors to be calibrated, and has strong scalability; it is insensitive to the accumulated errors of the sensors themselves, and has high calibration accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 A flow chart of a multi-sensor joint calibration method based on speed optimization provided by an embodiment of the present invention;

[0023] Figure 2 A schematic diagram of the structure of a multi-sensor joint calibration system based on speed optimization provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] The specific implementation modes of the present invention are further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0025] The problem to be solved by the present invention is that before the mobile robot performs positioning and navigation operations, it needs to calculate the conversion relationship between the coordinate systems of different sensors and the base coordinate system. The sensors in the present invention must be installed on the same mobile robot. The base coordinate system is the target coordinate system to which all sensors need to be converted. The base coordinate system can coincide with the coordinate system of a certain sensor. The initial value of the transformation from each sensor to the base coordinate system needs to be calibrated in advance. The accuracy of the initial value is relatively low. The initial value can be measured or estimated on a graph or a physical object. Assume that the set of m sensors and their initial transformation matrices to the base coordinate system is T_list = {T1, T2, …, T m}, the process of multi-sensor joint calibration method based on speed optimization is as follows Figure 1 As shown, the method specifically comprises the following steps:

[0026] S1, m sensors collect data and timestamps during the driving process of the mobile robot, calculate the speed of the sensor at the time of collection, and store the timestamp and corresponding speed in pairs in the data set Sensor_list;

[0027] IMU and odometer can obtain sensor speed through simple integration calculation of raw data. Vision (camera) and laser sensors need to calculate odometer posture through front-end mileage calculation method, and then calculate sensor speed through continuous time posture. The sensor speed v calculation formula is as follows:

[0028]

[0029] Among them, pose i+1 、pose i represents the continuous time t i+1 ,t i The posture below;

[0030] After completion, the data set of all sensors is obtained: Sensor_list = {S1, S2, ..., S m}, where S i Represents the data set of sensor i, expressed as represents sensor i at time t n speed.

[0031] S2. Obtain the speed values ​​of m sensors in the same time series through interpolation method and put them into the data set Sensor_list;

[0032] Since the data sampling frequencies of multiple sensors may be different, the collected data may not be at the same time. Therefore, it is necessary to interpolate the collected data to obtain the data at the same time. The method for obtaining the speed values ​​of m sensors in the same time series is as follows:

[0033] S21, putting the timestamp of each sensor into the timestamp container Time_all, arranging them in order, and deleting redundant timestamps;

[0034] S22, sequentially select the data set S of each sensor in the data set Sensor_list, and the data set S for sensor i i Do the following:

[0035] Traverse all timestamps t in the timestamp container Time_all, and check whether there is a timestamp with timestamp t in the data set S. If the detection result is no, calculate the speed value v of sensor i at timestamp t based on the interpolation method t , insert the data set S i middle;

[0036] In the embodiment of the present invention, in the data set S i Find two timestamps t adjacent to time t i ,t i+1 , the timestamp satisfies t i <t<t i+1 , the velocity value v of sensor i at timestamp t t The calculation formula is as follows:

[0037]

[0038] in, They represent sensor i at timestamp t i ,t i+1 The speed value below.

[0039] After completing the interpolation of all sensor data sets, the interpolated data sets of all sensors are obtained. in, Indicates t k The velocity values ​​of sensor 1, sensor 2, ..., sensor m in their own coordinate systems at each moment.

[0040] S3. Based on the principle that the velocity values ​​of different sensors in the base coordinate system are equal, a least squares problem is constructed. The transformation matrix of each sensor relative to the base coordinate system can be obtained by nonlinearly solving the least squares problem.

[0041] S31. The data set after all sensor interpolation is calculated, and the following least squares problem can be constructed:

[0042]

[0043] V t-i represents the velocity value of sensor i in its own coordinate at time t, T i is the transformation matrix of sensor i relative to the base coordinates, V t-j represents the velocity value of sensor j in its own coordinate at time t, T j is the transformation matrix of sensor j relative to the base coordinates,

[0044] The coordinate system of one of the sensors is used as the base coordinate system, that is, the velocity values ​​of each sensor at the same time should be equal after being converted to the base coordinate system. The least squares problem is solved in step S32 to calculate the precise transformation matrix T_list = {T1, T2, ..., T m}.

[0045] S32, nonlinear least squares solution:

[0046] 1) For the nonlinear least squares problem that needs to be solved:

[0047]

[0048]

[0049] Perform Taylor expansion on F(x) and take the first-order linear term approximation into the above formula:

[0050]

[0051] Where J(x) is the Jacobian matrix of F(x);

[0052] 2) Take the derivative of the above formula and set the reciprocal to 0:

[0053] J(x) T J(x)Δx=-J(x) T F(x)

[0054] Let A = J(x) T J(x),B=-J(x) T F(x), then the above formula can be simplified to AΔx=B. Solving this linear equation system, we can obtain the posture increment Δx;

[0055] 3) Add the current pose x to the pose increment Δx to get the optimized pose, and judge whether the components of Δx meet the constraints. If so, end the iteration and get the final optimized pose. If not, bring the optimized pose into step 3) to continue iterating and solving.

[0056] Figure 2 The schematic diagram of the structure of the multi-sensor joint calibration system based on speed optimization provided by the embodiment of the present invention, for the convenience of explanation, only shows the part related to the embodiment of the present invention, the system includes:

[0057] Set m sensors on the same mobile robot and a processing unit connected to the m sensors. The processing unit calibrates the transformation matrix T_list = {T1, T2, ..., T m}.

[0058] The present invention is described above by way of example in conjunction with the accompanying drawings. It is obvious that the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the protection scope of the present invention.

Claims

1. A multi-sensor joint calibration method based on speed optimization, characterized in that: The method specifically comprises the following steps: S1, m sensors collect data and timestamps during the mobile robot's driving process, and calculate the speed of the sensor at the time of collection; S2. Obtain the speed values ​​of m sensors in the same time series through interpolation method and put them into the data set Sensor_list; S3, converting the velocity values ​​of different sensors in the base coordinate system into a least squares problem, and solving the least squares nonlinearly to obtain the transformation matrix of each sensor relative to the base coordinate system; The least squares problem is formulated as follows: Among them, V t-i represents the velocity value of sensor i in its own coordinate at time t, T i is the transformation matrix of sensor i relative to the base coordinates, V t-j represents the velocity value of sensor j in its own coordinate at time t, T j is the transformation matrix of sensor j relative to the base coordinates; By solving the least squares problem nonlinearly, the precise transformation matrix T_list = {T1, T2, ..., T m }.

2. The multi-sensor joint calibration method based on speed optimization as claimed in claim 1, characterized in that: The method for obtaining the speed values ​​of the m sensors in the same time series is specifically as follows: S21, putting the timestamp of each sensor into the timestamp container Time_all, arranging them in order, and deleting redundant timestamps; S22, sequentially select the data set S of each sensor in the data set Sensor_list, interpolate the data set S, and obtain the speed values ​​of all sensors in the same time series, where the data set S of sensor i i The interpolation process is as follows: Traverse all timestamps t in the timestamp container Time_all, and check whether there is a timestamp identical to timestamp t in the data set S. If the detection result is no, calculate the speed value v of sensor i at timestamp t based on the interpolation method. t , insert the data set S i middle.

3. The multi-sensor joint calibration method based on speed optimization as claimed in claim 1, characterized in that: In the dataset S i Find two timestamps t adjacent to time t i ,t i+1 , where t i <t<t i+1 , the velocity value v of sensor i at timestamp t t The calculation formula is as follows: in, They represent sensor i at timestamp t i ,t i+1 The speed value below.

4. A multi-sensor joint calibration system based on speed optimization, characterized in that: The system comprises: m sensors are arranged on the same mobile robot, and a processing unit is connected to the m sensors in communication. The processing unit calibrates the transformation matrix T_list={T1, T2, …, T m }.

Images