CN114199281A - Multi-sensor combined calibration method and system based on speed optimization - Google Patents
Multi-sensor combined calibration method and system based on speed optimization Download PDFInfo
- Publication number
- CN114199281A CN114199281A CN202111521844.7A CN202111521844A CN114199281A CN 114199281 A CN114199281 A CN 114199281A CN 202111521844 A CN202111521844 A CN 202111521844A CN 114199281 A CN114199281 A CN 114199281A
- Authority
- CN
- China
- Prior art keywords
- sensor
- sensors
- speed
- data set
- time
- Prior art date
- Legal status (The legal status 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 status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000005457 optimization Methods 0.000 title claims abstract description 18
- 230000009466 transformation Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 7
- 238000004891 communication Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 3
- 239000000126 substance Substances 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a multi-sensor combined calibration method based on speed optimization, which comprises the following steps: s1, acquiring data and timestamps of the mobile robot in the driving process by the m sensors, and calculating the speed of the sensors at the acquisition time; s2, acquiring speed values of m sensors under the same time sequence through an interpolation method, and putting the speed values into a data set Sensor _ list; and S3, converting the speed values of different sensors under the base coordinate system into a least square problem, and solving the least square problem in a nonlinear way to obtain a transformation matrix of each sensor relative to the base coordinate system. The invention has no requirements on the type, performance and quantity of the sensors to be calibrated, and has strong expandability; the method is insensitive to the self accumulated error of the sensor and has high calibration precision.
Description
Technical Field
The invention belongs to the technical field of robots, and particularly relates to a multi-sensor combined calibration method and system based on speed optimization.
Background
With the development of science and technology, mobile robots are widely applied in the fields of warehouse logistics, intelligent inspection and the like, and the environment perception technology is an important component of the mobile robot technology and is the basis for the mobile robots to execute tasks. Depending on a single sensor to sense the environment, a satisfactory result cannot be obtained, so that a plurality of sensors are installed on the body of the mobile robot, information of various sensors is subjected to complementary and optimized combination processing, and finally an optimal observation result of the environment is generated, such as an IMU, a GPS, a camera, a laser radar and the like. Before a plurality of sensors are comprehensively applied, the relative pose transformation of each sensor needs to be known, namely the calibration of the sensor, and a transformation matrix from each sensor to a base coordinate system needs to be calibrated.
The existing calibration method calculates the positioning results of different sensors to obtain the relative relationship between the sensors, such as the calibration between a laser radar and a odometer or an IMU. However, the above calibration scheme has poor expansibility, cannot adapt to the variation of the type and number of the sensors, and the calibration result is affected by the accumulated error of the measurement of the sensors.
Disclosure of Invention
The invention provides a multi-sensor combined calibration method based on speed optimization, and aims to provide a multi-sensor calibration method with strong expansibility and high calibration precision.
The invention is realized in such a way that a multi-sensor combined calibration method based on speed optimization specifically comprises the following steps:
s1, acquiring data and timestamps of the mobile robot in the driving process by the m sensors, and calculating the speed of the sensors at the acquisition time;
s2, acquiring speed values of m sensors under the same time sequence through an interpolation method, and putting the speed values into a data set Sensor _ list;
and S3, converting the speed values of different sensors under the base coordinate system into a least square problem, and solving the least square problem in a nonlinear way to obtain a transformation matrix of each sensor relative to the base coordinate system.
Further, the speed value obtaining method of the m sensors under the same time sequence is specifically as follows:
s21, putting the Time stamp of each sensor into a Time stamp container Time _ all, arranging the Time stamps in sequence, and deleting redundant Time stamps;
s22, sequentially selecting the data set S of each Sensor in the data set Sensor _ list, interpolating the data set S, and acquiring the speed values of all the sensors under the same time sequence, wherein the data set S of the Sensor iiThe interpolation process of (2) is as follows:
traversing all timestamps t in a timestamp container Time _ all, detecting whether timestamps corresponding to the timestamps t exist in a data set S, and if the detection result is negative, calculating a speed value v of the sensor i under the timestamps t based on an interpolation methodtInserting the data set SiIn (1).
Further, in the data set SiFind two time stamps t adjacent to the time ti、ti+1Wherein t isi＜t＜ti+1Velocity value v of sensor i at time stamp ttThe calculation formula is as follows:
wherein the content of the first and second substances,
respectively indicating the time stamp t of the sensor ii、ti+1The lower velocity value.
Further, the formula of the least squares problem is as follows:
solving the least square problem through nonlinearity, and calculating to obtain a precise transformation matrix T _ list ═ T of each sensor relative to a base coordinate system1,T2,…,Tm}。
The invention is realized in such a way that a multi-sensor combined calibration system based on speed optimization comprises:
setting m sensors on the same mobile robot, and a processing unit in communication connection with the m sensors, wherein the processing unit calibrates a transformation matrix T _ list ═ T { T } of each sensor relative to a base coordinate system based on the multi-sensor combined calibration method based on speed optimization1,T2,…,Tm}。
The invention has no requirements on the type, performance and quantity of the sensors to be calibrated, and has strong expandability; the method is insensitive to the self accumulated error of the sensor and has high calibration precision.
Drawings
FIG. 1 is a flow chart of a multi-sensor joint calibration method based on speed optimization according to an embodiment of the present invention;
fig. 2 is a schematic structural diagram of a multi-sensor combined calibration system based on speed optimization according to an embodiment of the present invention.
Detailed Description
The following detailed description of the embodiments of the present invention will be given in order to provide those skilled in the art with a more complete, accurate and thorough understanding of the inventive concept and technical solutions of the present invention.
The invention aims to solve the problem that the mobile robot needs to calculate the conversion relation between the coordinate systems of different sensors and a base coordinate system before positioning, navigation and other operations. The sensors in the invention are required to be arranged on the same mobile robot, the base coordinate system is a target coordinate system to which all the sensors are required to be converted, the base coordinate system can be superposed with the coordinate system of a certain sensor, the initial value of the transformation from each sensor to the base coordinate system is required to be calibrated in advance, the accuracy of the initial value is relatively low, and the initial value can be obtained by measurement or estimation on a graph or a real object. Assume a set of m sensors and their initial transformation matrices to the base coordinate system, T _ list ═ T1,T2,…,TmFig. 1 shows a flow of a multi-sensor joint calibration method based on speed optimization, and the method specifically includes the following steps:
s1, acquiring data and timestamps of the mobile robot in the driving process by m sensors, calculating the speed of the sensors at the acquisition time, and storing the timestamps and the corresponding speed in a data set Sensor _ list in pairs;
the IMU and the odometer can obtain the sensor speed through simple integral calculation of original data, the vision (camera) and the laser sensor need to calculate the position and pose of the odometer through a front-end mileage calculation method, and then the speed of the sensor is calculated through the position and pose of continuous time, and the calculation formula of the sensor speed v is as follows:
wherein the positioni+1、poseiRepresenting successive time instants ti+1、tiA lower pose;
obtaining a data set of all sensors after completion, wherein the data set is S1,S2,…,SmIn which S isiData set representing sensor i, denoted as
Indicating sensor i at time tnThe speed of (2).
S2, acquiring speed values of m sensors under the same time sequence through an interpolation method, and putting the speed values into a data set Sensor _ list;
because the data sampling frequencies of the multiple sensors may be different, the acquired data may not be at the same time, and therefore interpolation calculation needs to be performed on the acquired data to obtain data at the same time, and the speed value obtaining method of the m sensors in the same time sequence is specifically as follows:
s21, putting the Time stamp of each sensor into a Time stamp container Time _ all, arranging the Time stamps in sequence, and deleting redundant Time stamps;
s22, sequentially selecting data setsData set S for each Sensor in Sensor _ list, data set S for Sensor iiThe following operations are performed:
traversing all timestamps t in a timestamp container Time _ all, detecting whether timestamps corresponding to the timestamps t exist in a data set S, and if the detection result is negative, calculating a speed value v of the sensor i under the timestamps t based on an interpolation methodtInserting the data set SiPerforming the following steps;
in the embodiment of the invention, in the data set SiFind two time stamps t adjacent to the time ti、ti+1The time stamp satisfies ti＜t＜ti+1Velocity value v of sensor i at time stamp ttThe calculation formula is as follows:
wherein the content of the first and second substances,
respectively indicating the time stamp t of the sensor ii、ti+1The lower velocity value.
After the interpolation of all the sensor data sets is completed, the interpolated data sets of all the sensors are obtained
Wherein the content of the first and second substances,
represents tkTime sensor 1, sensor 2 … …, and sensor m are speed values in their own coordinate system.
S3, constructing a least square problem based on the principle that the speed values of different sensors under the base coordinate system are equal, and obtaining a transformation matrix of each sensor relative to the base coordinate system by solving the least square nonlinearly.
S31, calculating to obtain a data set after interpolation of all sensors, and constructing a least square problem as follows:
Vt-irepresenting the speed value, T, of the sensor i in its own coordinates at time TiIs a transformation matrix of sensor i with respect to base coordinates, Vt-jRepresents the velocity value of the sensor j at time T in its own coordinate, TjIs the transformation matrix for sensor j relative to the base coordinate,
taking the coordinate system of one of the sensors as a base coordinate system, that is, the speed values of the sensors at the same time should be equal after being converted to the base coordinate system, solving the least square problem by using step S32, and calculating to obtain a precise transformation matrix T _ list ═ T of each sensor relative to the base coordinate system1,T2,…,Tm}。
S32, solving nonlinear least squares:
1) for a non-linear least squares problem to be solved:
taylor expansion is performed on F (x), taking the first order linear term approximation into the equation:
wherein J (x) is the Jacobian matrix of F (x);
2) the above equation is derived and the reciprocal is made to be 0:
J(x)TJ(x)Δx＝-J(x)TF(x)
let A be J (x)TJ(x),B＝-J(x)TF (x), the above equation can be simplified to a Δ x ═ B, and the linear equation set is solved, so that the pose increment Δ can be obtainedx；
3) And adding the current pose x to the current pose increment delta x to obtain an optimized pose, judging whether each component of the delta x meets constraint conditions, if so, ending iteration to obtain a final optimized pose, and if not, substituting the optimized pose into the step 3) to continue iterative solution.
Fig. 2 is a schematic structural diagram of a multi-sensor combined calibration system based on speed optimization according to an embodiment of the present invention, which is only shown in relevant parts according to the embodiment of the present invention for convenience of description, and the system includes:
the method comprises the steps that m sensors on the same mobile robot are arranged, a processing unit is in communication connection with the m sensors, and the processing unit calibrates a transformation matrix T _ list ═ T of each sensor relative to a base coordinate system based on data collected by the m sensors1,T2,…,Tm}。
The invention has been described above with reference to the accompanying drawings, it is obvious that the invention is not limited to the specific implementation in the above-described manner, and it is within the scope of the invention to apply the inventive concept and solution to other applications without substantial modification.
Claims (5)
1. A multi-sensor combined calibration method based on speed optimization is characterized by specifically comprising the following steps:
s1, acquiring data and timestamps of the mobile robot in the driving process by the m sensors, and calculating the speed of the sensors at the acquisition time;
s2, acquiring speed values of m sensors under the same time sequence through an interpolation method, and putting the speed values into a data set Sensor _ list;
and S3, converting the speed values of different sensors under the base coordinate system into a least square problem, and solving the least square problem in a nonlinear way to obtain a transformation matrix of each sensor relative to the base coordinate system.
2. The multi-sensor combined calibration method based on speed optimization as claimed in claim 1, wherein the speed value obtaining method of the m sensors under the same time sequence is specifically as follows:
s21, putting the Time stamp of each sensor into a Time stamp container Time _ all, arranging the Time stamps in sequence, and deleting redundant Time stamps;
s22, sequentially selecting the data set S of each Sensor in the data set Sensor _ list, interpolating the data set S, and acquiring the speed values of all the sensors under the same time sequence, wherein the data set S of the Sensor iiThe interpolation value process of (1) is as follows:
traversing all timestamps t in a timestamp container Time _ all, detecting whether timestamps corresponding to the timestamps t exist in a data set S, and if the detection result is negative, calculating a speed value v of the sensor i under the timestamps t based on an interpolation methodtInserting the data set SiIn (1).
3. The method for multi-sensor joint calibration based on speed optimization of claim 1, wherein the data set S is a data setiFind two time stamps t adjacent to the time ti、ti+1Wherein t isi＜t＜ti+1Velocity value v of sensor i at time stamp ttThe calculation formula is as follows:
wherein v isti、vti+1Respectively indicating the time stamp t of the sensor ii、ti+1The lower velocity value.
4. The multi-sensor joint calibration method based on speed optimization as claimed in claim 1, wherein the formula of the least square problem is as follows:
solving the least square problem through nonlinearity, and calculating to obtain a precise transformation matrix T _ list ═ T of each sensor relative to a base coordinate system1,T2,…,Tm}。
5. A multi-sensor joint calibration system based on speed optimization, the system comprising:
setting m sensors on the same mobile robot, processing unit connected with the m sensors in communication, the processing unit calibrating the transformation matrix T _ list ═ T of each sensor relative to the base coordinate system based on the multi-sensor combined calibration method based on speed optimization claimed in any one of claims 1 to 41,T2,…,Tm}。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521844.7A CN114199281A (en) | 2021-12-13 | 2021-12-13 | Multi-sensor combined calibration method and system based on speed optimization |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521844.7A CN114199281A (en) | 2021-12-13 | 2021-12-13 | Multi-sensor combined calibration method and system based on speed optimization |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114199281A true CN114199281A (en) | 2022-03-18 |
Family
ID=80653256
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111521844.7A Pending CN114199281A (en) | 2021-12-13 | 2021-12-13 | Multi-sensor combined calibration method and system based on speed optimization |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114199281A (en) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106251305A (en) * | 2016-07-29 | 2016-12-21 | 长春理工大学 | A kind of realtime electronic image stabilizing method based on Inertial Measurement Unit IMU |
| WO2017161608A1 (en) * | 2016-03-21 | 2017-09-28 | 完美幻境(北京)科技有限公司 | Geometric calibration processing method and device for camera |
| CN107443377A (en) * | 2017-08-10 | 2017-12-08 | 埃夫特智能装备股份有限公司 | Sensor robot coordinate system conversion method and Robotic Hand-Eye Calibration method |
| WO2018006521A1 (en) * | 2016-07-06 | 2018-01-11 | 深圳市元征科技股份有限公司 | Method and device for calibrating acceleration sensor |
| CN108225327A (en) * | 2017-12-31 | 2018-06-29 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of structure and localization method of top mark map |
| CN109544638A (en) * | 2018-10-29 | 2019-03-29 | 浙江工业大学 | A kind of asynchronous online calibration method for Multi-sensor Fusion |
| CN109975792A (en) * | 2019-04-24 | 2019-07-05 | 福州大学 | Method based on Multi-sensor Fusion correction multi-line laser radar point cloud motion distortion |
| CN112254729A (en) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | Mobile robot positioning method based on multi-sensor fusion |
| CN112762937A (en) * | 2020-12-24 | 2021-05-07 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 2D laser sequence point cloud registration method based on occupancy grid |
| CN113091771A (en) * | 2021-04-13 | 2021-07-09 | 清华大学 | Laser radar-camera-inertial navigation combined calibration method and system |
| WO2021174507A1 (en) * | 2020-03-05 | 2021-09-10 | 深圳市大疆创新科技有限公司 | Parameter calibration method, device, and system, and storage medium |
| CN113483755A (en) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | Multi-sensor combined positioning method and system based on non-global consistent map |
-
2021
- 2021-12-13 CN CN202111521844.7A patent/CN114199281A/en active Pending
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017161608A1 (en) * | 2016-03-21 | 2017-09-28 | 完美幻境(北京)科技有限公司 | Geometric calibration processing method and device for camera |
| WO2018006521A1 (en) * | 2016-07-06 | 2018-01-11 | 深圳市元征科技股份有限公司 | Method and device for calibrating acceleration sensor |
| CN106251305A (en) * | 2016-07-29 | 2016-12-21 | 长春理工大学 | A kind of realtime electronic image stabilizing method based on Inertial Measurement Unit IMU |
| CN107443377A (en) * | 2017-08-10 | 2017-12-08 | 埃夫特智能装备股份有限公司 | Sensor robot coordinate system conversion method and Robotic Hand-Eye Calibration method |
| CN108225327A (en) * | 2017-12-31 | 2018-06-29 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of structure and localization method of top mark map |
| CN109544638A (en) * | 2018-10-29 | 2019-03-29 | 浙江工业大学 | A kind of asynchronous online calibration method for Multi-sensor Fusion |
| CN109975792A (en) * | 2019-04-24 | 2019-07-05 | 福州大学 | Method based on Multi-sensor Fusion correction multi-line laser radar point cloud motion distortion |
| WO2021174507A1 (en) * | 2020-03-05 | 2021-09-10 | 深圳市大疆创新科技有限公司 | Parameter calibration method, device, and system, and storage medium |
| CN112254729A (en) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | Mobile robot positioning method based on multi-sensor fusion |
| CN112762937A (en) * | 2020-12-24 | 2021-05-07 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 2D laser sequence point cloud registration method based on occupancy grid |
| CN113091771A (en) * | 2021-04-13 | 2021-07-09 | 清华大学 | Laser radar-camera-inertial navigation combined calibration method and system |
| CN113483755A (en) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | Multi-sensor combined positioning method and system based on non-global consistent map |
Non-Patent Citations (2)
| Title |
|---|
| 杨杰;史震;岳鹏;程子健;: "三轴加速度计GFSINS安装误差标定与补偿方法", ***工程与电子技术, no. 04, 15 April 2011 (2011-04-15) * |
| 林家春;朱敏杰;李睿;李梦玮;: "基于激光跟踪仪点位测量的工业机器人位置稳定时间测量与分析", 机电工程, no. 06, 20 June 2018 (2018-06-20) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN111238535B (en) | IMU error online calibration method based on factor graph | |
| CN110987068B (en) | Data fusion method for multi-sensor integrated control system | |
| CN110442831B (en) | Space non-cooperative target space-based search method based on nonlinear deviation evolution | |
| CN110738275A (en) | UT-PHD-based multi-sensor sequential fusion tracking method | |
| CN117724035B (en) | Interferometer direction finding positioning method based on two-stage correction | |
| CN115290071A (en) | Relative positioning fusion method, device, equipment and storage medium | |
| CN113791240B (en) | Acceleration estimation method, system, equipment and medium based on high-order synovial membrane tracking differentiator | |
| CN109916417B (en) | Map establishing method, map establishing device, computer equipment and storage medium thereof | |
| CA2675310C (en) | Method for evaluation of sensor measured values | |
| CN114442133A (en) | Unmanned aerial vehicle positioning method, device, equipment and storage medium | |
| CN110677140A (en) | Random system filter containing unknown input and non-Gaussian measurement noise | |
| CN114199281A (en) | Multi-sensor combined calibration method and system based on speed optimization | |
| CN111121818B (en) | Calibration method for camera and two-dimensional code in unmanned vehicle | |
| CN109655057B (en) | Filtering optimization method and system for accelerator measurement value of six-push unmanned aerial vehicle | |
| CN116481543A (en) | Multi-sensor fusion double-layer filtering positioning method for mobile robot | |
| CN110849349A (en) | Fusion positioning method based on magnetic sensor and wheel type odometer | |
| CN112873280B (en) | Calibration method and device for sensor of robot | |
| CN112987054B (en) | Method and device for calibrating SINS/DVL combined navigation system error | |
| CN112965076B (en) | Multi-radar positioning system and method for robot | |
| CN111664882B (en) | Encoder position signal estimation method based on time stamp | |
| CN112489075A (en) | Sequence inertia type multi-sensor fusion filtering method based on characteristic function | |
| CN116774194A (en) | Laser radar calibration method based on deep learning | |
| CN112987010B (en) | System and method for multi-radar mapping of robot | |
| CN117665748A (en) | Laser radar odometer method using piecewise linear continuous time track |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |