JP2022039906A - Multi-sensor combined calibration device and method - Google Patents

Abstract

To provide a multi-sensor combined calibration device and method.SOLUTION: The multi-sensor combined calibration device comprises a robot arm; a sensor fusion frame is arranged on the robot arm; a laser radar, a monocular camera and a computer for processing data are arranged on the sensor fusion frame; it further comprises a laser radar-camera four-calibration-plate combined calibration target; the laser radar-camera four-calibration-plate combined calibration target comprises a first calibration plate, a second calibration plate, a third calibration plate and a fourth calibration plate; the central positions of the first calibration plate, the second calibration plate and the third calibration plate are marked by dots, which are used for providing feature points for external parameter calibration.SELECTED DRAWING: Figure 1

Description

The present invention relates to the field of sensor technology. In particular, it relates to a calibration technique using a multi-sensor.

Real-time localization and mapping (referred to as Simultaneous Localization and Mapping) technology provides environmental awareness information for unmanned driving, and conventional SLAM technology is divided into laser SLAM and visual SLAM. Laser radar has advantages such as high distance measurement accuracy and is not affected by light, and cameras have advantages such as low cost and abundant image information. However, the single sensor SLAM has great localization. For example, in the case of a laser radar, the update frequency is slow, there is motion distortion, and it is not possible to provide accurate measured values in a harsh environment such as rain or snow. In the case of a camera, accurate three-dimensional information cannot be obtained, and the limitation due to ambient light is relatively large.

Inertial navigation systems can provide accurate angular velocities and accelerations as attitude estimation aids. Therefore, the environmental perception ability of SLAM can be improved by fusing multi-sensor data such as a laser radar, a visual sensor, and an inertial navigation system.

Calibration of the sensors in the SLAM system is divided into internal parameter calibration and external parameter calibration. Sensor internal parameter calibration mainly refers to the calculation of the camera's internal parameter matrix and the error coefficient of the inertial navigation system to ensure the accuracy of the sensor measurement data. External parameter calibration between sensors is a prerequisite for accurate fusion of multi-sensor information, and external parameter calibration between sensors determines the attitude conversion relationship between the coordinate systems of each sensor. ..

In the conventional external parameter calibration method, the attitude conversion relationship from the coordinate system of the laser radar, the coordinate system of the camera, and the coordinate system of the inertial navigation system to the coordinate system of the vehicle body is obtained with reference to the coordinate system of the vehicle body. However, in the conventional external parameter calibration, all the sensors are fixed in the car body and the car body is limited in the motion dimension, so the calibration process is complicated and accurate calibration of yaw angle and roll angle is realized. It's difficult to do.

Laser radar has high ranging accuracy and has no special requirements for calibration reference, but since the camera is based on 2D image characteristics, a specific calibration reference is required for calibration. Currently, existing laser radar and camera external parameter calibration methods include a single grid calibration plate-based calibration method, an "L" -shaped calibration plate-based calibration method, and three-dimensional calibration methods. A calibration method based on a grid target is included. These methods are similar, all of which are to solve the external parameter matrix by matching the 3D feature points of the laser with the 2D feature points of the camera.

The laser radar and inertial navigation system must be calibrated under motion conditions, and the inertial navigation system can provide accurate acceleration and angular velocity measurements. The conventional calibration method is a hand-eye calibration method, but it is difficult to guarantee the accuracy of the calibration. Baidu's Apollo calibration tool in China enables sensor data collection and external parameter calibration by controlling the vehicle to move around an "8" shape.

Combined calibration with multiple sensors is currently one of the hottest topics in the field of unmanned driving. The existing calibration technology has disadvantages such as low degree of automation, complicated operation, and low accuracy.

The present invention has been made to solve the above-mentioned problems existing in the prior art, and an object of the present invention is to provide a composite calibration device and method using a multi-sensor, which is easy to operate and has high accuracy.

The object of the present invention can be achieved through the following technical solutions.

A sensor fusion frame is installed on the robot arm including the robot arm, a laser radar, a monocular camera, and a computer for data processing are installed on the sensor fusion frame, and a four calibration plate by a laser radar-camera is installed. Further including a composite calibration target, said laser radar-camera four calibration plate composite calibration target includes a No. 1 calibration plate, a No. 2 calibration plate, a No. 3 calibration plate, and a No. 4 calibration plate. The center positions of the No. 1 calibration plate, No. 2 calibration plate, and No. 3 calibration plate are marked with dots and are used to provide the feature points of the external parameter calibration. Black and white grid patterns are arranged for internal parameter calibration of the camera, four calibration plates are arranged in the shape of "ta", and No. 1 calibration plate and No. 2 calibration plate are arranged in parallel. The No. 3 calibration plate and the No. 4 calibration plate are placed in parallel in front of the No. 1 calibration and the No. 2 calibration plate, and the No. 3 calibration plate and the No. 4 calibration plate are the No. 1 calibration plate. Lower than the No. 2 calibration plate, the angle difference between the normal vector n3 on the surface of the No. 3 calibration plate and the normal vector n4 on the surface of the No. 4 calibration plate is larger than 30 °, and the surface of the No. 1 calibration plate Provided is a multi-sensor composite calibration device in which the angle difference between the normal vector n1 and the normal vector n2 on the surface of the No. 2 calibration plate is larger than 30 °.

Provided is a multi-sensor composite calibration method including internal parameter calibration of a camera and external parameter calibration of a laser radar-camera. In the case of camera internal parameter calibration, the computer unit controls the robot arm and camera so that the camera shoots against the grid calibration plate under different postures, and Zhang Zhengyou calibration method is adopted. The internal parameter K of the camera is calculated. For laser radar-camera external parameter calibration, it involves the following steps:

Step 1. The computer unit adjusts the posture of the robot arm so that the calibration plate module appears in the field of view of the laser radar and the camera, and controls the robot arm to keep it stationary. Collect.

は、第ｉ番目のキャリブレーションプレートの中心点位置のレーザーレーダの座標系における三次元座標値であり、

をマッチングのためのレーザー特徴点として選定する。 Step 2. Analyze and process the laser radar data, extract the point cloud feature points, record the coordinate information of each point cloud, filter the abnormal points, and adopt the point cloud division method to obtain the point cloud data. Is divided into four different groups {L ₁ }, {L ₂ }, {L ₃ }, {L ₄ }, and the point cloud data of the four calibration plates is divided into four different groups {L 1}, {L 3}, {L 4}, and the point cloud is used by the K-means method. Extract the clustering center point of

Is the three-dimensional coordinate value in the laser radar coordinate system at the center point position of the i-th calibration plate.

Is selected as a laser feature point for matching.

Step 3, analyze and process the camera data, extract visual feature points, record the gray value of each pixel, adopt the FAST keypoint extraction algorithm, and reveal the gray value of the local pixel of the calibration plate. By detecting the location changed to, the center point position of each calibration plate is extracted, and the dot center point positions of the No. 1 calibration plate, No. 2 calibration plate, and No. 3 calibration plate are extracted, and the dot center point positions are extracted. By recording the coordinate values C ₁ , C ₂ , and C ₃ and analyzing the relationship between the grids, the center point coordinate value C ₄ of the No. 4 calibration plate is obtained, and C _i is the i-th calibration. It is a two-dimensional coordinate value in the camera coordinate system C at the center point position of the plate, and _Ci is selected as a visual feature point for matching.

及びカメラの特徴点

によってマッチング関係

を確立し、特徴点のマッチング関係によって最小再投影誤差を確立し、誤差方程式を構築し、誤差方程式によって最小二乗法による求解を行い、最適な外部パラメータ行列Ｔ_ＬＣを取得する。 Step 4, features of laser radar

And camera features

Matching relationship by

Is established, the minimum reprojection error is established by the matching relationship of the feature points, the error equation is constructed, the solution is performed by the least squares method by the error equation, and the optimum external parameter matrix _TLC is obtained.

In some embodiments, the edge positions of the calibration plate are collected and used as feature points based on the center position feature points of the calibration plate.

In some embodiments, the internal parameter calibration of the camera employs a grid calibration method, in which the camera captures the grid calibration plate under different postures. The computer unit controls the robot arm and the camera, extracts the edge information of 20 to 30 images, and uses the ZhangZhengyou calibration method to calculate the internal parameter K of the camera. be.

Some embodiments further include an inertial navigation system located on the sensor fusion frame. The combined calibration method includes a combined calibration method using a laser radar and an inertial navigation system. The initial motion time of the robot arm is defined as t ₀ , the motion end time of the robot arm is defined as t _n , the laser point group scanned at t _n time is defined as P _n , and the laser radar at t ₀ time. The coordinate system of is defined as L ₀ , the coordinate system of the inertial navigation system at t ₀ time is defined as I ₀ , the coordinate system of the laser radar at t _n time is defined as L _n , and at t _n time. The coordinate system of the inertial navigation system is defined as In, the attitude conversion matrix between t ₀ time and t _n time of the laser radar is defined as T _L , and the inertial navigation system _is defined from t ₀ time to t _n time. The attitude conversion matrix between is defined as _{TI, and the external parameter matrix between the laser radar and the inertial navigation system is defined as TLI .}

の初期時刻ｔ_０での座標系Ｌ_０と運動終了時刻での座標系Ｌ_ｎとの間の姿勢変換行列Ｔ_Ｌを計算する。姿勢変換行列Ｔ_Ｌの計算方法は、下記の通りである。反復最近傍法を採用して第ｉフレーム点群と第ｉ＋１フレーム点群をマッチングし、第ｉフレーム点群

と第ｉ＋１フレーム点群

のマッチング関係を取得する。第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換は、回転行列Ｒ_ｉと並進ベクトルｔ_ｉから構成されており、２フレーム点群の姿勢変換関係を示す。さらに、誤差方程式を構築し、誤差方程式を最小二乗問題に変換し、ＳＶＤ法を採用してＲ_ｉ、ｔ_ｉを計算する。Ｒ_ｉ、ｔ_ｉによって第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換行列を

として取得する。さらに、ｎフレームのレーザーレーダの姿勢変換行列Ｔ_ｉを累積的に乗算し、レーザーレーダのｔ_０時刻からｔ_ｎ時刻までの姿勢変換行列Ｔ_Ｌを取得する。
ステップ４、慣性ナビゲーションシステムの座標系

の初期時刻ｔ_０での座標系Ｉ_０と運動終了時刻ｔ_ｎでの座標系Ｉ_ｎとの間の姿勢変換行列Ｔ_Ｉを計算する。姿勢変換行列Ｔ_Ｉの計算方法は、下記の通りである。慣性ナビゲーションシステムの加速度測定データ及び角速度測定データに対して積分を行い、変位データｔ_Ｉ及び回転データＲ_Ｉを取得すると、慣性ナビゲーションシステムのｔ_０時刻からｔ_ｎ時刻までの姿勢変換行列

に表される。
ステップ５、Ｔ_Ｌによってｔ_ｎ時刻でのレーザー点群Ｐ_ｎをｔ_０時刻でのＬ_０座標系に投影し、点群Ｐ_ｎＬを取得する。
ステップ６、Ｔ_Ｉ及びキャリブレーション待ちパラメータＴ_ＬＩによってｔ_ｎ時刻でのレーザー点群Ｐ_ｎをｔ_０時刻でのＬ_０座標系に投影し、Ｐ_ｎＩを取得する。
ステップ７、２グループの点群Ｐ_ｎＬ及びＰ_ｎＩをマッチングする。２グループの点群を整列し、外部パラメータ行列Ｔ_ＬＩに対して最適化を行う。反復最近傍法を採用してＰ_ｎＬ及びＰ_ｎＩが記述された同一点群領域をレジストレーションし、最近傍誤差Ｔ_{ｅｒｒｏｒ}を構築及び最適化し、Ｔ_{ｅｒｒｏｒ}、Ｔ_Ｌ、Ｔ_Ｉによって外部パラメータ行列Ｔ_ＬＩを求解する。 The combined calibration method includes the following steps.
Step 1. The robot arm moves according to a designated trajectory, and the laser radar and the inertial navigation system collect data during the movement process of the robot arm.
Step 2. The robot arm is controlled to stop the movement, and the data collected by the sensor is processed. In the case of laser radar, distortion removal processing is performed on the laser point cloud for each frame according to the uniform motion model. A point cloud filtering algorithm is used to remove outliers in the point cloud data. To reduce the complexity of the calculation, the voxel grid method is used to downsample the laser point cloud data for each frame.
Step 3, laser radar coordinate system

The posture transformation matrix TL between the coordinate system L ₀ at the initial time t ₀ and the coordinate system _{L n} at the end time of the exercise is calculated. The calculation method of the attitude transformation matrix _TL is as follows. The iterative nearest neighbor method is used to match the i-frame point cloud and the i + 1 frame point cloud, and the i-frame point cloud is used.

And i + 1 frame point cloud

Get the matching relationship of. The attitude conversion of the laser radar from the i-th time to the _i + 1-th time is composed of the rotation matrix R _i and the translation vector ti, and shows the attitude conversion relationship of the two-frame point cloud. Furthermore, an error equation is constructed, the error equation is converted into a least squares problem, and the SVD method is adopted to calculate _Ri and _ti . The attitude transformation matrix of the laser radar from the _i -th time to the i + 1-time by R _i and ti

Get as. Further, the attitude transformation matrix Ti of the _n -frame laser radar is cumulatively multiplied to obtain the attitude transformation matrix _TL from the t ₀ time to the t _n time of the laser radar.
Step 4, Coordinate system of inertial navigation system

The posture transformation matrix TI between the coordinate system _{I 0} at the initial time t ₀ and the coordinate system In _n at the exercise end time t _n is calculated. The calculation method of the attitude transformation matrix _TI is as follows. When the acceleration measurement data and the angular velocity measurement data of the inertial navigation system are integrated and the displacement data t _I and the rotation data _RI are acquired, the attitude transformation matrix from the t ₀ time to the t _n time of the inertial navigation system is obtained.

It is represented by.
Step 5, the laser point cloud P _n at t _n time is projected onto the _{L 0} coordinate system at t ₀ time by TL, and the point cloud P _n L is acquired.
Step 6, TI and the calibration waiting parameter T _LI project the laser point cloud P _n at t _n time onto the L ₀ coordinate system at t ₀ time, and acquire P _{n I.}
Steps 7 and 2 are matched with the point clouds P _nL and P _nI . Two groups of point clouds are aligned and optimized for the external parameter matrix _TLI . The iterative nearest neighbor method is used to resist the same point group region in which P _nL and P _nI are described, to construct and optimize the nearest neighbor _{error Terrar} , and to use the _Terrar , T _L , and TI to construct and optimize the external parameter matrix T. _{Solve LI} .

In some embodiments, if the visual conditions are sufficient, the observation data of the monocular camera is recorded, a visual-IMU calibration tool is employed, and an external parameter matrix _TCI between the monocular camera and the inertial navigation system is provided. calculate. Combined verification is performed based on the attitude consistency between the determined three external parameter matrices _TLC , _TLI , and _TCI , adjustments are made to the conversion parameters in the external parameter matrix, and combined calibration by the multi-sensor is performed. Improve accuracy. The parameter optimization employs an online adjustment method, fuses data from a laser radar, a monocular camera, and an inertial navigation system, and makes adjustments for _TLI and _TCI .

Compared with the prior art, the multi-sensor composite calibration device and method of the present invention has the following advantages.

The present invention is applied to visual sensors such as multi-line laser radars such as 16 lines, 32 lines and 64 lines, monocular cameras, binocular cameras and RGBD cameras. An embodiment of the present invention constructs a portable multi-sensor fusion flare that is convenient for calibration and secondary development. In the embodiment of the present invention, intelligent calibration and batch calibration can be realized by using the auxiliary calibration method using a robot arm.

In drawings (not necessarily drawn to a certain scale), similar reference numbers may describe similar components in different figures. Similar reference numbers with different letter suffixes can represent different examples of similar components. The drawings show, by way of example, each embodiment discussed herein.
It is a schematic diagram of the robot arm in which the sensor fusion frame is installed. It is a schematic diagram of a multi-sensor fusion frame. It is a schematic diagram of the positional relationship of the four calibration plates composite calibration target by the laser radar-camera. It is a schematic diagram of the spatial arrangement of a calibration target. It is a schematic diagram of the principle of the combined calibration of the laser radar-camera. It is a distribution schematic diagram of a feature point. It is a schematic diagram of a laser-visual feature matching. It is a schematic diagram of the attitude change of a laser radar. It is a schematic diagram of the attitude change of an inertial navigation system. It is a flowchart of calibration.

The following are specific examples of the present invention, further describing the technical solutions of the present invention while combining the drawings, but the present invention is not limited to these examples, and the following embodiments are: , The present invention according to the claims is not limited. Also, all combinations of features described in the embodiments are not necessarily essential to the solution of the present invention.

Example A multi-sensor composite calibration device including a multi-sensor fusion frame 101, a robot arm 102, a coupling mechanism 103, a computer unit 104, and an operation platform, wherein the multi-sensor fusion frame 101 is a multi-sensor composite construction method. The laser radar 201, monocular camera 202, and inertial navigation system 203 are fixed to a freely movable metal frame 204, which is mounted via a jig 206 for environmental sensing of unmanned vehicles, unmanned aircraft, etc. It is useful for performing external parameter calibration as well as being applied to secondary development. The metal frame has a length of 18 cm, a width of 6 cm, and a height of 8 cm. Each sensor is installed according to the same coordinate system orientation by adopting a multi-sensor composite construction method. Among them, the laser radar 201 is installed at the center position of the upper part of the metal frame 204, and the fixing device 205 is installed inside the metal frame. The monocular camera 202 is installed at a position 5 cm to the left of the fixing device 205, and the inertial navigation system 203 is installed at a position 5 cm to the right of the fixing device 205. The robot arm 102 is installed above the operating platform 105 to provide triaxial translational and rotational movements. The end of the robot arm is connected to the fixing device 205 via the connecting mechanism 103, and the height of the multi-sensor fusion frame 101 from the ground is 140 cm under the initial state. The computer unit 103 has a function of controlling the motion of the bot arm 102, controlling data collection by a sensor, processing sensor data, and calculating an external parameter matrix.

A laser radar-camera four calibration plate composite calibration target is further included, and a laser radar camera four calibration plate composite calibration target is composed of four calibration plates of the same size. Four calibration plates by the laser radar-camera The composite calibration target 104 is composed of four 30 cm × 30 cm calibration plates. As shown in FIG. 3, the central position of the Nos. 1 to 3 calibration plates is marked with dots to provide feature points for external parameter calibration. On the No. 4 calibration plate, black and white grid patterns are arranged for internal parameter calibration of the camera. The four calibration plates are placed in the air in the shape of a "field". In order to divide the four calibration plates more accurately, they are placed in the environment at different distances and angles. As shown in FIG. 4, the No. 3 calibration plate and the No. 4 calibration plate adopt and fix a base having a height of 120 cm, the base is arranged on the horizon, and the distance from the multi-sensor fusion frame is D ₁ . = 130 cm, and the angle difference between the normal vector n ₃ of the No. 3 calibration plate and the normal vector n ₄ of the No. 4 calibration plate is 30 ° or more. To ensure that the No. 1 and No. 2 calibration plates are not covered, the No. 1 and No. 2 calibration plates are fixed by adopting a base with a height of 160 cm, and the base is on the horizon. The distance from the multi-sensor fusion frame is D ₂ = 180 cm. The angle difference between the normal vector n ₁ of the No. 1 calibration plate and the normal vector n ₂ of the No. 2 calibration plate is 30 ° or more.

In the internal parameter calibration of the camera, the computer unit controls the robot arm and the camera so that the camera shoots against the grid calibration plate under different postures, and edge information of 20 to 30 images is obtained. It is extracted and the internal parameter K of the camera is calculated by adopting the Zhang Zhengyou calibration method.

The four calibration plate composite calibration target by the laser radar camera is adopted for the internal parameter calibration of the monocular camera and the external parameter calibration of the laser radar camera. The combined calibration method using the laser radar and the monocular camera includes the following steps.

First, as shown in FIG. 5, the computer unit adjusts the posture of the robot arm so that the composite calibration target appears in the field of view of the laser radar and the camera. After controlling the robot arm to keep it stationary and using a laser radar and camera to collect data, the computer unit processes the measurement data. Finally, we construct the least squares problem and solve the external parameter matrix _TLC .

Laser radar data and camera data are processed by collecting the data.

は、第ｉ番目のキャリブレーションプレートの中心点位置のレーザーレーダの座標系Ｌにおける三次元座標値である。

をマッチングのためのレーザー特徴点として選定する。 In the case of laser radar data, the coordinate information of each point cloud is recorded, the abnormal points are filtered, the point cloud data is divided by adopting the point cloud division method, and the point cloud data of 4 calibration plates are 4 different. Divide into groups {L ₁ }, {L ₂ }, {L ₃ }, {L ₄ }. The clustering center point of the point cloud is extracted using the K-means method.

Is a three-dimensional coordinate value in the coordinate system L of the laser radar at the center point position of the i-th calibration plate.

Is selected as a laser feature point for matching.

は、第ｉ番目のキャリブレーションプレートの中心点位置のカメラ座標系Ｃにおける二次元座標値である。Ｃ_ｉをマッチングのための視覚特徴点として選定する。
レーザーレーダの三次元特徴点

及び単眼カメラの二次元特徴点

によってマッチング関係

を確立する。
図７に示すように、レーザーレーダ座標

及び単眼カメラ座標

の特徴点Ｐｉに基づくマッチング関係を定義することにより、最小再投影誤差を確立し、最小二乗問題を構築する。レーザーレーダ及びカメラの外部パラメータ行列を次のように定義する。

For camera data, the gray value of each pixel is recorded, the FAST keypoint extraction algorithm is adopted, and the location where the gray value of the local pixel of the calibration plate is clearly changed is detected, so that the gray value of each calibration plate is recorded. Extract the center point position. The center positions of the dots on the No. 1 to No. 3 calibration plates are extracted, and the coordinate values C ₁ , C ₂ , and C ₃ are recorded. In the case of the No. 4 calibration plate, the center point coordinate value C ₄ of the calibration plate is obtained by analyzing the relationship between the grids.

Is a two-dimensional coordinate value in the camera coordinate system C at the center point position of the i-th calibration plate. Select _Ci as a visual feature point for matching.
3D feature points of laser radar

And 2D feature points of monocular cameras

Matching relationship by

To establish.
As shown in FIG. 7, laser radar coordinates

And monocular camera coordinates

By defining the matching relationship based on the feature point Pi, the minimum reprojection error is established and the least squares problem is constructed. The external parameter matrix of the laser radar and camera is defined as follows.

Here, R is a rotation matrix and t is a translation vector. Combining the matching relation and the external parameter matrix, the error equation can be expressed as follows.

Here, _si is the depth value of the visual feature point, and the least squares optimization function is constructed according to the error equation so that the error is minimized, and the optimum external parameter matrix is obtained.

Further, if necessary, more feature points are collected and matched, and the edge positions of the calibration plate are collected and used as feature points based on the center position feature points of the calibration plate. As shown in FIG. 6, five positions for each calibration plate are selected as feature points, a least squares optimization function is constructed according to the above method, and an external parameter matrix is solved. Experiments have shown that the larger the number of characteristic points, the more accurate the calculation of the external parameter matrix.

をそれぞれ計算し、偏差の大きい中心点座標値を削除し、平均値策略を採用して次のように残りの中心点座標を計算する。

Furthermore, the angle of the calibration plate is adjusted, the sensor data of the calibration plates of multiple groups are collected under the state of different angles, and the center point coordinate value of the calibration plate of the data for each group is collected.

Are calculated respectively, the center point coordinates with a large deviation are deleted, and the mean value strategy is adopted to calculate the remaining center point coordinates as follows.

With existing calibration techniques, the position and angle of the calibration plate is fixed. Due to the sparseness of the laser radar's vertical point cloud, it is difficult to ensure that scanning by the laser radar completely covers the entire area of the calibration plate. If the scanning line by the laser radar cannot completely scan the complete contour of the calibration plate, the coordinates of the point cloud feature point (center point of the calibration plate) will be significantly different from the actual position.

The method adjusts the angle of the calibration plate so that the laser radar and camera collect multiple groups of calibration plate data. Since the center point of the calibration plate is adopted as the rotation center of the angle adjustment, the center point coordinates do not always change regardless of the adjustment of the calibration plate. This method uses the invariance of the center point position of the calibration plate to fit the center point coordinates of multiple groups with the point cloud data of multiple groups under different angles, and removes the center point coordinates with a large error. The average value of the remaining center point coordinates is used as the point cloud feature point of the calibration plate. Compared with the existing technology, the accuracy of the feature points extracted by this method is greatly improved.

The combined calibration method of the laser radar and the inertial navigation system consists of the following steps.

With the support of the robot arm, we adopt external parameter calibration by laser radar-inertial navigation system based on point cloud matching. The specific steps are as follows.

First, the robot arm is controlled to move, and the sensor moves in space to collect data.

Next, the movement of the robot arm is stopped, and processing is performed on the data collected by the sensor.

After that, the attitude transformation matrix _TL between the coordinate system L ₀ at the initial time t ₀ of the laser radar and the coordinate system L _n at the motion end time t _n is calculated. The attitude transformation matrix TI between the coordinate system I ₀ at the initial time t ₀ of the inertial navigation system and the coordinate system _{I n} at the motion end time t _n is calculated.

Again, the laser point cloud P _n at t _n time is projected onto the _{L 0} coordinate system at t ₀ time by TL, and the point cloud P _n L is acquired. The laser point cloud P _n at t _n time is projected onto the L ₀ coordinate system at t ₀ time by _TI and the calibration waiting parameter T _LI , and P n _I is acquired. Finally, the external parameter matrix _TLI is calculated by matching the two groups of point clouds P _nL and P _nI and aligning the two groups of point clouds.

The movement of the robot arm is to control the robot arm so as to move within a designated locus. With respect to the coordinate axis of the robot arm, it moves 100 cm along the positive direction of the X axis, moves 100 cm along the negative direction of the X axis, moves 100 cm along the positive direction of the Y axis, and moves in the negative direction of the Y axis. It moves 100 cm along the positive direction of the Z axis, 100 cm along the negative direction of the Z axis, and 100 cm along the negative direction of the Z axis. Rotate 180 ° clockwise around the X axis, 180 ° counterclockwise around the X axis, 180 ° clockwise around the Y axis, 180 ° counterclockwise around the Y axis ° Rotate, rotate 180 ° clockwise around the Z axis, and 180 ° counterclockwise around the Z axis. A laser radar and an inertial navigation system collect data during the movement of the robot arm.

In the case of a laser radar, the sensor data processing is performed by performing distortion removal processing on the laser point cloud for each frame according to a uniform motion model, adopting a point cloud filtering algorithm, and removing outliers of the point cloud data. In order to reduce the complexity of the calculation, the laser point cloud data for each frame is downsampled using the voxel grid method.

と第ｉ＋１フレーム点群

とのマッチング関係を取得する。第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換が回転行列Ｒ_ｉと並進ベクトルｔ_ｉから構成されると定義すると、２フレーム間の対応する関係は次のように表される。

The calculation method of the posture transformation matrix _TL is as follows. The iterative nearest neighbor method is used to match the i-frame point cloud and the i + 1 frame point cloud, and the i-frame point cloud is used.

And i + 1 frame point cloud

Get the matching relationship with. If we define that the attitude transformation of the laser radar from the i-th time to the _i + 1-time is composed of the rotation matrix R _i and the translation vector ti, the corresponding relationship between the two frames is expressed as follows.

The error equation is expressed as follows.

By constructing the least squares problem, the SVD method is adopted to calculate _Ri and _ti .

The attitude conversion matrix of the laser radar from the i-th time to the i + 1 time is as follows.

_TL is a posture transformation matrix from the t ₀ time to the t _n time of the laser radar, and is expressed as follows.

The calculation method of the attitude transformation matrix _TI is as follows. The acceleration measurement data and the angular velocity measurement data of the inertial navigation system are integrated, and the displacement data t _I and the rotation data _RI are acquired. The attitude transformation matrix of the inertial navigation system from t ₀ time to t _n time is expressed as follows.

Projecting the laser point cloud P _n at the t _n time onto the L ₀ coordinate system at the t ₀ time means that in the case of a laser radar, as shown in FIG. 8, the _L of P _n is directly measured by the attitude transformation matrix TL. The coordinate display P _nL in the ₀ coordinate system can be acquired.

In the case of the inertial navigation system, as shown in FIG. 9, the coordinate display P n _I in the L ₀ coordinate system of P _n can be acquired by the attitude transformation matrix _TI and the external parameter matrix T _LI .

The calculation of the external parameter matrix _TLI is to align the point cloud P _nL and the point cloud P _nI . Theoretically, P _nL and point cloud P _nI describe point cloud data in the same region, and they overlap in space. That is, it is as follows.

Due to the existence of external parameter errors, P _nL and P _nI do not completely overlap. The iterative nearest neighbor method is adopted to resist the point cloud region of the same region described by P _nL and P _nI , and to construct and optimize the nearest neighbor error.

The external parameter matrix _TLI is solved by _Terror , T _L , and TI.

Up to this point, the external parameter matrices _TLC and _TLI are acquired by the above calibration method.

The present invention is a laser radar-inertial navigation calibration based on point group registration, and the existing technology (laser radar / IMU combined calibration method based on point group matching) is a method of mounting a laser radar and an IMU on a small vehicle. However, since small vehicles can only move in a plane, they cannot provide full six-degree-of-freedom movement. In this method, since the robot arm is adopted as the motion device of the calibration system, abundant spatial motion can be provided. The accuracy of calibration is improved as compared with the prior art.

In addition, if necessary, if the visual conditions are sufficient, the observation data of the monocular camera is recorded, a visual-IMU calibration tool is adopted, and an external parameter matrix _TCI between the monocular camera and the inertial navigation system is obtained. calculate. Combined verification is performed based on the attitude consistency between the determined three external parameter matrices _TLC , _TLI , and _TCI , and if the verification fails, the conversion parameters in the external parameter matrix are adjusted until the verification is successful. do. Therefore, if the verification fails, the embodiment of the present invention optimizes the parameters for _{TLC, TLI ,} and _TCI to improve the accuracy of the combined calibration by the multi-sensor. The parameter optimization employs an online adjustment method, fuses data from a laser radar, a monocular camera, and an inertial navigation system, and makes adjustments for _TLI and _TCI .

In addition, if necessary, the robot arm is controlled so that the calibration target appears within the field of view of the camera if the visual conditions are sufficient. The robot arm is controlled to move in six degrees of freedom in space under the condition that the camera can always observe the calibration target. The observation data of the monocular camera is recorded and a calibration tool is used to calibrate the external parameter matrix _TCI of the visual-inertial navigation system.

Furthermore, compound optimization is performed for the attitude transformation matrix between the laser radar, the monocular camera, and the inertial measurement unit. A composite verification is performed based on the attitude consistency between the determined three external parameter matrices _TLC , _TLI , and _TCI . In principle, the following mathematical relationship exists between the three external parameter matrices.

However, due to the error in the calibration process, it is not possible to actually achieve complete posture consistency. Therefore, it is necessary to optimize the conversion parameters in the external parameter matrix and improve the accuracy of the combined calibration by the multi-sensor. _{TLC, TLI ,} and _TCI are adjusted by minimizing the attitude conversion total error.

The specific steps of optimizing the parameters are as follows.

Calculate each attitude conversion error.

がすべて誤差閾値ｅｒｒｏｒ_ｔｈｒより小さい場合、Ｔ_ＬＣ、Ｔ_ＬＩ、及びＴ_ＣＩ間は既に姿勢一貫性を満たしていると見なされ、調整不要となる。

If are all smaller than the error threshold error _thr , it is considered that the posture consistency is already satisfied between _TLC , _TLI , and _TCI , and no adjustment is required.

After that, adjustments are made to each posture transformation matrix so that the total error error _T is minimized under the condition that all the posture transformation matrix errors are smaller than the error threshold.

の大きさを判断し、３つの誤差の大きさから主な誤差由来を初歩的に特定し、誤差の正負から調整する方向を初歩的に決定する。

Judging the magnitude of the error, the main error origin is elementaryly identified from the magnitudes of the three errors, and the direction of adjustment is elementaryly determined from the positive and negative of the error.

The translation vector and each vector component in the rotation matrix are analyzed, and the following adjustments are made to the translation vector.

の具体的なベクトル成分を分析することによって取得する。回転行列に対して、次のような調整を行う。

Here, the magnitudes and positive / negative of Δx, Δy, and Δz are

Obtained by analyzing the specific vector components of. Make the following adjustments to the rotation matrix.

の具体的なベクトル成分を分析することによって取得する。 Here, the magnitude and positive / negative of Δr are

Obtained by analyzing the specific vector components of.

An example will be given.

である場合、Ｔ_ＬＣが最大の誤差由来と見なされる。Ｔ_ＬＣの誤差項目

及び

を分析する。 First, identify the origin of the main error. The order of the magnitude of the error is

If, the _TLC is considered to be from the largest error. Error item of _TL C

as well as

To analyze.

However, the cause of error is not limited to _TLC . In the case of T _LI and T _CI , adjustments are made by adopting the same steps as above. The difference is that the step sizes of α, β, and γ are adjusted (α>β> γ) for T _LC , T _LI , and T _CI , respectively, and the maximum adjustment range is limited.

During the optimization process, the step sizes of α, β, and γ are appropriately changed by adjusting T _LC , T _LI , and T _CI in order and observing the change of error _T. If it is observed that the error _T is clearly decreased during the adjustment process, the step size is appropriately increased, and if the error _T is gradually decreasing or increasing during the adjustment process, the step size is appropriately increased. Or change the positive or negative of the step size.

Adjust the external parameter matrix according to the above steps until error _T is minimized.

In order to prevent the calculation result from becoming a local optimization during the adjustment process, the adjustment order of TLC, _TLI , and _TCI is changed, and the optimization result is used as the above method until the minimum _{eraser T} is found. Compare.

Compared with the existing technology, the composite optimization method of the attitude transformation matrix by the multi-sensor of the present invention adopts the offline optimization method, and is based on the attitude consistency between _{TLC, TLI ,} and _TCI . Posture transformation matrix Constructs an optimization function for the total error and adjusts the parameters for each external parameter. This optimization method achieves the effect of minimizing the error of each external parameter matrix and maximizing the accuracy.

Although some terms are often used herein, it does not preclude the possibility of using other terms. These terms are used only to more conveniently describe and explain the essence of the invention. Interpreting them as any additional limitation is contrary to the spirit of the invention. Unless otherwise specified, the order of execution of operations, steps, etc. in the apparatus and method shown in the specification and drawings shall be in any order unless the output of the previous process is used in the subsequent process. Can be done. For convenience of explanation, explanations using "first", "next", etc. do not necessarily mean that they should be executed in this order.

The particular embodiments described herein are merely exemplary of the spirit of the invention. One of ordinary skill in the art may make various modifications or supplements or replacements with similar methods for the particular embodiments described, which deviate from the spirit of the invention or, by the claims of the attachment. It does not exceed the defined range.

101, sensor fusion frame; 102, robot arm; 103, coupling mechanism; 104, computer unit; 105, operation platform; 201, laser radar; 202, monocular camera; 203, inertial navigation system; 204, metal frame; 205, fixed Equipment; 206, jig.

Claims (9)

It is a multi-sensor composite calibration device that includes a robot arm, a sensor fusion frame is installed on the robot arm, and a laser radar, a monocular camera, and a computer for data processing are installed on the sensor fusion frame. hand,
The four calibration plate composite calibration target by the laser radar-camera further includes the four calibration plate composite calibration target by the laser radar camera, and the four calibration plate composite calibration target is the No. 1 calibration plate, the No. 2 calibration plate, and the No. 3 calibration plate. , Including No. 4 calibration plate,
The center positions of the No. 1 calibration plate, No. 2 calibration plate, and No. 3 calibration plate are marked with dots and are used to provide features for external parameter calibration.
On the No. 4 calibration plate, black and white grid patterns are arranged for internal parameter calibration of the camera.
The four calibration plates are arranged in the shape of a "field", the No. 1 calibration plate and the No. 2 calibration plate are arranged in parallel, and the No. 3 calibration plate and the No. 4 calibration plate are the No. 1 calibration. And placed in parallel in front of the No. 2 calibration plate, the No. 3 and No. 4 calibration plates are lower than the No. 1 and No. 2 calibration plates.
The angle difference between the normal vector n3 on the surface of the No. 3 calibration plate and the normal vector n4 on the surface of the No. 4 calibration plate is larger than 30 °.
The angular difference between the normal vector n1 on the surface of the No. 1 calibration plate and the normal vector n2 on the surface of the No. 2 calibration plate is greater than 30 °.
Composite calibration device with multiple sensors. The composite calibration method of the composite calibration device using the multi-sensor according to claim 1.
Includes camera internal parameter calibration and laser radar-camera external parameter calibration,
In the case of camera internal parameter calibration, the computer unit controls the robot arm and camera so that the camera shoots against the grid calibration plate under different postures, and the camera adopts the Zhang Masatomo calibration method. Calculate the internal parameter K and
Laser Radar-For external parameter calibration of the camera, including the following steps,
a) The computer unit adjusts the posture of the robot arm so that the calibration plate module appears in the field of view of the laser radar and the camera.
The robot arm is controlled to keep it stationary, and the laser radar and camera collect data.
b) The laser radar data is analyzed and processed to extract point cloud feature points.
The coordinate information of each point cloud is recorded, the abnormal points are filtered, the point cloud data is divided by the point cloud division method, and the point cloud data of the four calibration plates are divided into four different groups {L ₁ }, Divide into {L ₂ }, {L ₃ }, and {L ₄ }, and use the K-means method to extract the clustering center point of the point cloud.

Is the three-dimensional coordinate value in the laser radar coordinate system at the center point position of the i-th calibration plate.

Is selected as a laser feature point for matching,
c) The camera data is analyzed and processed to extract visual feature points.
Extract the center point position of each calibration plate by recording the gray value of each pixel and using the FAST keypoint extraction algorithm to detect where the gray value of the local pixel of the calibration plate has changed significantly. death,
Extract the dot center point positions of the No. 1 calibration plate, No. 2 calibration plate, and No. 3 calibration plate, and record the coordinate values C ₁ , C ₂ , and C ₃ .
By analyzing the relationship between the grids, the center point coordinate value C4 of the No. ₄ calibration plate was obtained.
C _i is a two-dimensional coordinate value in the camera coordinate system C at the center point position of the i-th calibration plate, and C _i is selected as a visual feature point for matching.
d) Features of laser radar

And camera features

Matching relationship by

Established,
The minimum reprojection error is established by the matching relationship of the feature points, the error equation is constructed, the solution is performed by the least squares method by the error equation, and the optimum external parameter matrix _TLC is obtained.
A multi-sensor composite calibration method characterized by this. Based on the center position feature point of the calibration plate, the edge position of the calibration plate is collected and used as the feature point.
The combined calibration method using a multi-sensor according to claim 2. Adjust the angle of the calibration plate, collect the sensor data of the calibration plates of multiple groups under the condition of different angles, and the center point coordinate value of the calibration plate of the data for each group.

, Delete the center point coordinates with large deviations, and use the mean strategy to calculate the remaining center point coordinates.
The combined calibration method using a multi-sensor according to claim 2. The internal parameter calibration of the camera uses a grid calibration method.
In the grid calibration method, the computer unit controls the robot arm and the camera so that the camera shoots at the grid calibration plate under different postures, and the edge information of 20 to 30 images is extracted. Then, the internal parameter K of the camera is calculated by adopting the calibration method of Masatomo Zhang.
The combined calibration method using a multi-sensor according to claim 2. The composite calibration method of the composite calibration device using the multi-sensor according to claim 1.
Further includes an inertial navigation system located on the sensor fusion frame,
The combined calibration method includes a combined calibration method using a laser radar and an inertial navigation system.
The initial motion time of the robot arm is defined as t ₀ , the motion end time of the robot arm is defined as t _n , the laser point group scanned at t _n time is defined as P _n , and the laser radar at t ₀ time. The coordinate system of is defined as L ₀ , the coordinate system of the inertial navigation system at t ₀ time is defined as I ₀ , the coordinate system of the laser radar at t _n time is defined as L _n , and at t _n time. The coordinate system of the inertial navigation system is defined as In, the attitude conversion matrix between t ₀ time and t _n time of the laser radar is defined as T _L , and the inertial navigation system _is defined from t ₀ time to t _n time. If the attitude conversion matrix between is defined as _{TI and the external parameter matrix between the laser radar and the inertial navigation system is defined as TLI ,}
The combined calibration method comprises the following steps:
a) The robot arm moves according to a specified trajectory, and the laser radar and inertial navigation system collect data during the movement process of the robot arm.
b) Control the robot arm to stop the movement, process the data collected by the sensor, and
In the case of laser radar, distortion removal processing is performed on the laser point cloud for each frame according to the uniform motion model.
Uses a point cloud filtering algorithm to remove outliers in point cloud data.
To reduce the complexity of the calculation, the voxel grid method is used to downsample the laser point cloud data for each frame.
c) Laser radar coordinate system

Calculate the posture transformation matrix TL between the coordinate system _{L 0} at the initial time t ₀ and the coordinate system L _n at the end time of the exercise.
The calculation method of the attitude transformation matrix _TL is as follows.
The iterative nearest neighbor method is used to match the i-frame point cloud and the i + 1 frame point cloud, and the i-frame point cloud is used.

And i + 1 frame point cloud

Get the matching relationship of
The attitude transformation of the laser radar from the i-th time to the _i + 1th time is composed of the rotation matrix R _i and the translation vector ti, shows the attitude transformation relationship of the two-frame point group, and further constructs the error equation. The error equation is converted into the least squared problem, the SVD method is used to calculate R _i and ti, and the attitude conversion matrix of the laser radar from the _{i time to the i + 1 time is calculated by R i and ti} .

And the attitude transformation matrix Ti of the laser radar of _n frames is cumulatively multiplied to acquire the attitude transformation matrix _TL from t ₀ time to t _n time of the laser radar.
d) Coordinate system of inertial navigation system

Calculate the posture transformation matrix TI between the coordinate system _{I 0} at the initial time t ₀ and the coordinate system In _n at the exercise end time t _n .
The calculation method of the attitude transformation matrix _TI is as follows.
When the acceleration measurement data and the angular velocity measurement data of the inertial navigation system are integrated and the displacement data t _I and the rotation data _RI are acquired, the attitude transformation matrix from the t ₀ time to the t _n time of the inertial navigation system is obtained.

Represented in
e) The laser point cloud P _n at t _n time is projected onto the _{L 0} coordinate system at t ₀ time by TL, and the point cloud P _n L is acquired.
f) The laser point cloud P _n at t _n time is projected onto the L ₀ coordinate system at t ₀ time by _TI and the calibration waiting parameter T _LI , and P n _I is acquired.
g) Matching the two groups of point clouds P _nL and P _nI , aligning the two groups of point clouds, optimizing for the external parameter matrix _TLI , and adopting the iterative nearest neighbor method, P _nL and P. The point cloud region of the same region in which _nI is described is registered, the nearest neighbor _{error Terror} is constructed and optimized, and the external parameter matrix _TLI is solved by Terror, _TL , and _TI .
A multi-sensor composite calibration method characterized by this. If the visual conditions are sufficient, the observation data of the monocular camera is recorded, the visual-IMU calibration tool is adopted, and the external parameter matrix _TCI between the monocular camera and the inertial navigation system is calculated.
Combined verification is performed based on the attitude consistency between the determined three external parameter matrices _TLC , _TLI , and _TCI , and the conversion parameters in the external parameter matrix are adjusted to improve the accuracy of the combined calibration by the multi-sensor. death,
The parameter optimization employs an online adjustment method, fuses data from a laser radar, a monocular camera, and an inertial navigation system, and makes adjustments for _TLI and _TCI .
The combined calibration method using a multi-sensor according to claim 6. If the visual conditions are sufficient, control the robot arm so that the calibration target appears within the field of view of the camera, and control the robot arm in space under conditions where the camera can always observe the calibration target. Exercise with six degrees of freedom,
Record the observation data of the monocular camera and use a calibration tool to calibrate the external parameter matrix _TCI of the visual-inertial navigation system as follows:

The combined calibration method using a multi-sensor according to claim 7. Composite optimization is performed for the attitude transformation matrix between the laser radar, monocular camera, and inertial measurement unit.
Perform composite verification based on _postural consistency between the determined three external parameter matrices TLC, _TLI , and _TCI .
The specific steps are as follows:
Calculate each posture conversion error,

If are all less than the error threshold error _thr , it is considered that the posture consistency is already satisfied between _TLC , _TLI , and _TCI , and no adjustment is required.
Then, adjustments are made to each posture transformation matrix so that the total error error _T is minimized under the condition that all the posture transformation matrix errors are smaller than the error threshold.

Judging the magnitude of the error, the main error origin is elementaryly identified from the magnitudes of the three errors, and the direction of adjustment is elementaryly determined from the positive and negative of the error.
Analyze each vector component in the translation vector and rotation matrix, and make the following adjustments to the translation vector.

Here, the magnitudes and positive / negative of Δx, Δy, and Δz are

Obtained by analyzing the concrete vector components of
Make the following adjustments to the rotation matrix,

Here, the magnitude and positive / negative of Δr are

Obtained by analyzing the concrete vector components of,
The combined calibration method using a multi-sensor according to claim 7.

Images