CN103049912B - Random trihedron-based radar-camera system external parameter calibration method - Google Patents

Abstract

The invention discloses a random trihedron-based radar-camera system external parameter calibration method. According to the method, an external parameter of a system can be solved with only two frames of data by using a trihedron scene a natural environment. The method comprises the following steps of: stipulating a world coordinate system by using a trihedron; performing planar fitting on the trihedron observed in a radar system to obtain a parameter of each plane and solving conversion relation of the world coordinate system and the radar coordinate system and relative motion between the two frames of data under the radar coordinate system; and in a camera system, solving an essential matrix by using matching characteristic points extracted by front and rear frames, then solving relation motion under the camera coordinate system, solving the plane parameter under the camera coordinate system by using the parameter under the radar coordinate system, finally solving an external parameter of a radar-camera, and performing final optimization by using coplanarity of points on corresponding planes under the two coordinate systems. A scene required by the method is simpler; and the method has the characteristics of high anti-interference performance, simple experimental equipment and high flexibility.

Description

A kind of radar based on any trihedral-camera system method for calibrating external parameters

Technical field

The present invention relates to the method for radar-camera system calibrating external parameters, specifically a kind of radar based on any trihedral-camera system method for calibrating external parameters.

Background technology

Being a underlying issue in mobile robot field to the real-time structure of terrain environment, in order to realize this purpose, usually adopting binocular camera system or a radar sensor to provide three-dimensional information.But binocular camera system is owing to needing to carry out fine and close Stereo matching to carry out three-dimensional reconstruction, consuming time comparatively large, easily by external environment influence, so cannot meet the requirement of real-time and accuracy; Although radar sensor can provide real-time three-dimensional information, precision is higher, lacks the colouring information of environment.Therefore, more and more researcher formation radar-camera system that radar and monocular camera combined carrys out the environmental map that real-time providing has color and three-dimensional information.

In recent years, many researchers also provide the method for some radars-camera system calibrating external parameters.Document 1 (Zhang, Q.; Pless, R, " Extrinsic calibration of a camera and laser range finder (improvecamera calibration) ", IEEE International Conference on Intelligent Robots and Systems, IROS, 2004, pp.2301 – 2306.) and document 2 (Unnikrishnan, R.; Hebert, M., " Fast extrinsic calibration ofa laser rangefinder to a camera ", Technical report, Carnegie Mellon University, Rotbotics Institute, 2005.) gridiron pattern scaling board is utilized to carry out computing system external parameter, angle point on gridiron pattern will on camera image manual extraction, and need to gather multiple image, the method depends on the extraction order of accuarcy of angle point to a great extent, also there are certain requirements illumination.Document 3 (Rodriguez F, S.; Fremont, V.; Bonnifait, P, " Extrinsic calibration between a multi-layer lidar and a camera ", IEEE International Conferenceon Multisensor Fusion and Integration for Intelligent Systems, 2008, pp.214 – 219.) and document 4 (Li, G.; Liu, Y.; Dong, L.; Cai, X.; Zhou, D, " An algorithm for extrinsic parameters calibration of acamera and a laser range finder using line features ", IROS, 2007, pp.3854 – 3859.) in order to reduce the error of angle point grid, make use of the demarcation object of special shape, in radar data and view data, extract the corresponding corner location of demarcating thing, but the accuracy that the non-compactness of radar data itself determines the method can not be too high simultaneously.Recently, document 5 (Pandey, G., McBride, J.R., Savarese, S., Eustice, R.M., " Automatictargetless extrinsic calibration of a 3d lidar and camera by maximizing mutual information ", Proceedings of the AAAI National Conference on Artificial Intelligence, 2012.) any scene is not relied on, the three-dimensional point reflection strength only utilizing radar to provide and the mutual information of camera image are to optimize the external parameter of radar-camera system, but not all radar can provide the reflection strength of object, and the reflection strength of unlike material object is not identical.

Summary of the invention

In order to radar-camera data is effectively combined, the object of the present invention is to provide a kind of radar based on any trihedral-camera system method for calibrating external parameters, namely solve the position transformational relation of radar system and camera system; Utilize any trihedral common in natural scene, do not rely on the accuracy of manual input information, and only need two frame data can carry out external parameter fast to solve.

The step of the technical solution used in the present invention is as follows:

(1) utilize radar-camera system, two frame data are gathered to the demarcation scene that there is any trihedral, and according to trihedral definition world coordinate system O _w-X _wy _wz _w, there is relative motion between two frame data of collection, and the coloured image of the three-dimensional point cloud having corresponding radar to gather in every frame data and collected by camera;

(2) radar system parameters is solved: in three-dimensional point cloud, mark the point on trihedral three faces, be expressed as:

$P_l=\{P_{l_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_{l_i}^j\}$

Wherein subscript l represents these points at radar fix system O _l-X _ly _lz _lunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth point, and the number of each upper point is point parameter on same plane represent:

$N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j=0$

Wherein for the normal vector of each, for the distance to coordinate origin;

Then according to the coplanarity of trihedral at radar two frame data mid point, to two frames totally 6 faces carry out nonlinear optimization;

Pass through obtain rotation matrix and translation vector that world coordinates in two frame data is tied to radar fix system and obtain the kinematic parameter of radar-camera system under radar fix system between two frames thus, namely the first frame is relative to the rotation matrix of the second frame and translation vector

(3) camera system parameters is solved: utilize the corresponding region in SIFT algorithm trihedral three faces in front and back two two field picture of collected by camera to extract the unique point of front and back frame coupling, be expressed as:

$P_c=\{P_{c_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_c^j\}$

Wherein subscript c represents these points at camera coordinates system O _c-X _cy _cz _cunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth unique point, and the front and back two obverse matching characteristic of frame is counted out and is equally

According to P _ccalculate the essential matrix E of two frame motions, and obtain the kinematic parameter of radar-camera system under camera coordinates system between two frames further wherein for the first frame under camera coordinates system is relative to the rotation matrix of the second frame, for translation vector normalized result:

$t_{c_1{c}_2}=T_{c_1{c}_2}/\left|\right|T_{c_1{c}_2}\left|\right|,\left|\right|t_{c_1{c}_2}\left|\right|=1$

Utilize parameter under initialization camera coordinates system , herein for the parameter of each plane of trihedral under camera coordinates system; According to the corresponding projection relation of the unique point extracted, right be optimized;

Pass through obtain world coordinates in two frame data and be tied to the transformational relation of camera coordinates system, be i.e. rotation matrix and translation vector

(4) external parameter is solved: utilize R _wc, T _wc, R _wl, T _wlobtain the transformational relation that radar fix is tied to camera coordinates system, i.e. rotation matrix R _lcwith translation vector T _lc; Utilize coplanarity trihedral put under radar fix system and camera coordinates system, to R _lc, T _lccarry out nonlinear optimization, the transformational relation R after being optimized _lc, T _lc.

The concrete grammar that described step (2) solves radar system parameters is:

Utilize linear least-squares optimization object function F ₁, to the point on an i-th frame data jth face carry out plane, obtain fitting parameter

$F_1(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j\left|\right|}^2$

Pass through obtain the transformational relation that world coordinates is tied to radar fix system

$\left\{\begin{array}{cc}{r}_{\mathrm{wl}}1=\frac{N_l^1\×N_l^3}{\left|\right|N_l^1\×N_l^3\left|\right|}& \\ r_{\mathrm{wl}}3=N_l^3,& T_l={\left[\begin{array}{ccc}{N}_l^1& N_l^2& N_l^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_l^1& d_l^2& d_l^3\end{array}\right]}^T\\ r_{\mathrm{wl}}2=r_{\mathrm{wl}}3\×r_{\mathrm{wl}}1& \end{array}\right.$

Here r _wl1, r _wl2, r _wl3 are respectively R _wlcolumn vector;

Pass through to obtain under radar fix system the first frame relative to the kinematic parameter of the second frame

$\left\{\begin{array}{c}{R}_{l_1{l}_2}=R_{{\mathrm{wl}}_2}^{-1}{R}_{{\mathrm{wl}}_1}\\ T_{l_1{l}_2}=T_{{\mathrm{wl}}_2}-R_{l_1{l}_2}{T}_{{\mathrm{wl}}_1}\end{array}\right.$

According to the coplanarity that corresponding surface in two frame data is put, to all carry out nonlinear optimization, objective function is expressed as:

$F_2(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_2}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_1}^{\mathrm{jT}}(R_{l_2{l}_1}{P}_{l_2}^{j,k}+T_{l_2{l}_1})-d_{l_1}^j\left|\right|}^2+\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_1}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_2}^{\mathrm{jT}}(R_{l_1{l}_2}{P}_{l_1}^{j,k}+T_{l_1{l}_2})-d_{l_2}^j\left|\right|}^2$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector; Utilize after optimizing right upgrade.

The concrete grammar that described step (3) solves camera system parameters is:

Two-dimensional points P on image _cwith camera coordinates system lower peripheral surface point P _s,ctransformational relation be:

P _c＝G(P _s,c)

G () is corresponding projection function herein, is determined by known camera type and internal reference;

According to the epipolar geometry constraints of front and back frame corresponding point, obtain:

G ^-1(P _s,c1) ^TEG ^-1(P _s,c2)＝0

Here E is essential matrix; Existing maturation method is utilized to solve essential matrix, and the motion under decompositing camera coordinates system between two frames wherein for normalized translation vector:

$t_{c_1{c}_2}=T_{c_1{c}_2}/\left|\right|T_{c_1{c}_2}\left|\right|,\left|\right|t_{c_1{c}_2}\left|\right|=1$

Because the positional distance of radar and camera in radar-camera system is comparatively near, the rotation between coordinate system is less, so according to parameter to be asked under parameter initial estimation camera coordinates system under radar fix system:

$\left\{\begin{array}{c}\{N_{c_i}^j,d_{c_i}^j\}=\{N_{l_i}^j,d_{l_i}^j\}\\ \left|\right|T_{c_1{c}_2}\left|\right|=\left|\right|T_{l_1{l}_2}\left|\right|\end{array}\right.$

Then, matching characteristic point on image is utilized to utilize re-projection error after recovering three-dimensional coordinate on another two field picture is optimized

$F_3(N_{c_1},d_{c_1},N_{c_2},d_{c_2},R_{c_1{c}_2},T_{c_1{c}_2})=\arg \underset{R_{c_1{c}_2},T_{c_1{c}_2}}{\underset{N_{c_2},d_{c_2}}{\underset{N_{c_1},d_{c_1}}{\min }}}\underset{k=1}{\overset{n_c^j}{\mathrm{\Σ}}}\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\begin{array}{c}{|p_{c_2}^{j,k}-G(R_{c_1{c}_2}\frac{d_{c_1}{G}^{-1}\left(p_{c_1}^{j,k}\right)}{N_{c_1}^{j,T}{G}^{-1}\left(p_{c_1}^{j,k}\right)}+T_{c_1{c}_2})|}^2\\ {|p_{c_1}^{j,k}-G(R_{c_2{c}_1}\frac{d_{c_2}{G}^{-1}\left(p_{c_2}^{j,k}\right)}{N_{c_2}^{j,T}{G}^{-1}\left(p_{c_2}^{j,k}\right)}+T_{c_2{c}_1})|}^2\end{array}$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector;

Pass through obtain the transformational relation that world coordinates is tied to camera coordinates system

$\left\{\begin{array}{cc}{r}_{\mathrm{wc}}1=\frac{N_c^1\×N_c^3}{\left|\right|N_c^1\×N_c^3\left|\right|}& \\ r_{\mathrm{wc}}3=N_c^3,& T_c={\left[\begin{array}{ccc}{N}_c^1& N_c^2& N_c^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_c^1& d_c^2& d_c^3\end{array}\right]}^T\\ r_{\mathrm{wc}}2=r_{\mathrm{wc}}3\×r_{\mathrm{wc}}1& \end{array}\right.$

Here r _wc1, r _wc2, r _wc3 are respectively R _wccolumn vector.

The concrete grammar that described step (4) solves external parameter is:

Pass through in any one group of data, solve the transformational relation R between radar and camera coordinates system _lc, T _lc:

$\left\{\begin{array}{c}{R}_{\mathrm{lc}}={R_{{\mathrm{wc}}_i}R}_{{\mathrm{wl}}_i}^{-1}\\ T_{\mathrm{lc}}=T_{{\mathrm{wc}}_i}-R_{{\mathrm{wc}}_i}{R}_{{\mathrm{wl}}_i}^{-1}{T}_{{\mathrm{wl}}_i}\end{array},,,i,=,\mathrm{1,2}\right.$

Further, coplanarity trihedral corresponding surface put under radar fix system and camera coordinates system is utilized, according to objective function F ₄to R _lc, T _lccarry out nonlinear optimization, the R after being optimized _lc, T _lc.

$F_4(R_{\mathrm{lc}},T_{\mathrm{lc}})=\arg \underset{R_{\mathrm{lc}},T_{\mathrm{lc}}}{\min }\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{c_i}^{\mathrm{jT}}(R_{\mathrm{lc}}{P}_{l_i}^{j,k}+T_{\mathrm{lc}})-d_{c_i}^j\left|\right|}^2$

The beneficial effect that the present invention has is:

The present invention has taken into account noiseproof feature, operation complexity requirement is low, does not need to do special setting to environment, is suitable for the external conditions such as arbitrary illumination, weather; Scene of the presently claimed invention is fairly simple, common, has strong interference immunity, experimental facilities is simple, dirigibility is stronger feature concurrently.

Accompanying drawing explanation

Fig. 1 is overview flow chart of the present invention.

Fig. 2 is three coordinate system definition and relation schematic diagram in radar-camera system calibration process.

Embodiment

Below in conjunction with the drawings and specific embodiments, the present invention will be further described.

Fig. 1 gives the techniqueflow of radar-camera system method for calibrating external parameters.

Radar-camera system calibrating external parameters comprises following four parts: 1. gather two frame radar-camera system data; 2. radar system parameters is solved: utilize coordinate trihedral face put under radar system, solve the parameter of each plane in two frame radar datas, relative movement parameters and the transformational relation with world coordinate system; 3. solve camera system parameters: according to known camera parameter, utilize parameter under radar system, solve the parameter of each plane in two frame camera datas, relative movement parameters and the transformational relation with world coordinate system; 4. external parameter is solved: the transformational relation solving radar and camera according to the relativeness of radar and camera coordinates system and world coordinate system, the i.e. external parameter of radar-camera system, and utilize the coplanarity put under different sensors system on same plane to carry out final optimization pass.

1, two frame radar-camera system data are gathered:

As shown in Figure 2, according to the trihedral definition world coordinate system O in scene _w-X _wy _wz _w:

With the intersection point in trihedral three faces for initial point O _w, plane 1 is X with the intersection of plane 3 _waxle, the normal vector direction of plane 3 is Z _waxle, then can determine Y according to right-handed system rule _wdirection.

Utilize radar-camera system, two frame data are gathered to demarcation scene, and have relative motion between collection two frame data, require that two sensors can observe each face of trihedral simultaneously.

2, radar system parameters is solved:

In radar data, mark out the point on trihedral three faces, be expressed as:

$P_l=\{P_{l_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_{l_i}^j\}---\left(1\right)$

Wherein subscript l represents these points at radar fix system O _l-X _ly _lz _lunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth point, and the number of each upper point is point on same plane can use parameter represent:

$N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j=0---\left(2\right)$

Wherein for the normal vector of each, for the distance to coordinate origin.

Utilize linear least-squares optimization object function F ₁, to the point on an i-th frame data jth face carry out plane fitting, obtain parameter

$F_1(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j\left|\right|}^2---\left(3\right)$

Pass through the transformational relation that world coordinates is tied to radar fix system can be obtained $R_{{\mathrm{wl}}_i},T_{{\mathrm{wl}}_i},i=\mathrm{1,2}:$

$\left\{\begin{array}{cc}{r}_{\mathrm{wl}}1=\frac{N_l^1\×N_l^3}{\left|\right|N_l^1\×N_l^3\left|\right|}& \\ r_{\mathrm{wl}}3=N_l^3,& T_l={\left[\begin{array}{ccc}{N}_l^1& N_l^2& N_l^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_l^1& d_l^2& d_l^3\end{array}\right]}^T\\ r_{\mathrm{wl}}2=r_{\mathrm{wl}}3\×r_{\mathrm{wl}}1& \end{array}\right.---\left(4\right)$

Here r _wl1, r _wl2, r _wl3 are respectively R _wlcolumn vector.Pass through the first frame can be obtained under radar fix system relative to the kinematic parameter of the second frame

$\left\{\begin{array}{c}{R}_{l_1{l}_2}=R_{{\mathrm{wl}}_2}^{-1}{R}_{{\mathrm{wl}}_1}\\ T_{l_1{l}_2}=T_{{\mathrm{wl}}_2}-R_{l_1{l}_2}{T}_{{\mathrm{wl}}_1}\end{array}\right.---\left(5\right)$

According to the coplanarity that corresponding surface in two frame data is put, to all carry out nonlinear optimization, then utilize after optimizing right upgrade.

The objective function optimized is:

$F_2(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_2}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_1}^{\mathrm{jT}}(R_{l_2{l}_1}{P}_{l_2}^{j,k}+T_{l_2{l}_1})-d_{l_1}^j\left|\right|}^2+\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_1}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_2}^{\mathrm{jT}}(R_{l_1{l}_2}{P}_{l_1}^{j,k}+T_{l_1{l}_2})-d_{l_2}^j\left|\right|}^2---\left(6\right)$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector, with there is relation:

$R_{l_2{l}_1}=R_{l_1{l}_2}^{-1},T_{l_2{l}_1}=-R_{l_1{l}_2}^{-1}{T}_{l_1{l}_2}---\left(7\right)$

3, parameter under camera coordinates system is solved:

According to SIFT algorithm, (specific implementation is see document 6 (Lowe, David G. (1999). " Object recognition fromlocal scale-invariant features " .Proceedings of the International Conference on Computer Vision.2.pp.1150 – 1157.)) carry out feature point extraction, coupling front and back two two field picture is obverse, be expressed as:

$P_c=\{P_{c_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_c^j\}---\left(8\right)$

Wherein subscript c represents these points at camera coordinates system O _c-X _cy _cz _cunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth unique point, and the front and back two obverse matching characteristic of frame is counted out and is equally

Two-dimensional points P on image _cwith camera coordinates system lower peripheral surface point P _s,ctransformational relation be:

P _c＝G(P _s,c) (9)

G () is corresponding projection function herein, is determined by known camera type and internal reference.

According to the epipolar geometry constraints of front and back frame corresponding point, can obtain:

G ^-1(P _s,c1) ^TEG ^-1(P _s,c2)＝0 (10)

Here E is essential matrix.Document 7 (R.I.Hartley and A.Zisserman can be utilized, " MultipleView Geometry in Computer Vision ", Cambridge University Press, ISBN:0521540518,2004,2nd ed.) middle 8 ripe methods solve essential matrix, and the motion under decompositing camera coordinates system between two frames wherein for normalized translation vector:

$t_{c_1{c}_2}=T_{c_1{c}_2}/\left|\right|T_{c_1{c}_2}\left|\right|,\left|\right|t_{c_1{c}_2}\left|\right|=1---\left(11\right)$

Because the positional distance of radar and camera in radar-camera system is comparatively near, the rotation between coordinate system is less, so according to parameter to be asked under parameter initial estimation camera coordinates system under radar fix system:

$\left\{\begin{array}{c}\{N_{c_i}^j,d_{c_i}^j\}=\{N_{l_i}^j,d_{l_i}^j\}\\ \left|\right|T_{c_1{c}_2}\left|\right|=\left|\right|T_{l_1{l}_2}\left|\right|\end{array}\right.---\left(12\right)$

Then, matching characteristic point on image is utilized to utilize re-projection error after recovering three-dimensional coordinate on another two field picture is optimized

$F_3(N_{c_1},d_{c_1},N_{c_2},d_{c_2},R_{c_1{c}_2},T_{c_1{c}_2})=\arg \underset{R_{c_1{c}_2},T_{c_1{c}_2}}{\underset{N_{c_2},d_{c_2}}{\underset{N_{c_1},d_{c_1}}{\min }}}\underset{k=1}{\overset{n_c^j}{\mathrm{\Σ}}}\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\begin{array}{c}{|p_{c_2}^{j,k}-G(R_{c_1{c}_2}\frac{d_{c_1}{G}^{-1}\left(p_{c_1}^{j,k}\right)}{N_{c_1}^{j,T}{G}^{-1}\left(p_{c_1}^{j,k}\right)}+T_{c_1{c}_2})|}^2\\ {|p_{c_1}^{j,k}-G(R_{c_2{c}_1}\frac{d_{c_2}{G}^{-1}\left(p_{c_2}^{j,k}\right)}{N_{c_2}^{j,T}{G}^{-1}\left(p_{c_2}^{j,k}\right)}+T_{c_2{c}_1})|}^2\end{array}---\left(12\right)$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector, with there is relation:

$R_{c_2{c}_1}=R_{c_1{c}_2}^{-1},T_{c_2{c}_1}=-R_{c_1{c}_2}^{-1}{T}_{c_1{c}_2}---\left(13\right)$

Pass through the transformational relation that world coordinates is tied to camera coordinates system can be obtained

$\left\{\begin{array}{cc}{r}_{\mathrm{wc}}1=\frac{N_c^1\×N_c^3}{\left|\right|N_c^1\×N_c^3\left|\right|}& \\ r_{\mathrm{wc}}3=N_c^3,& T_c={\left[\begin{array}{ccc}{N}_c^1& N_c^2& N_c^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_c^1& d_c^2& d_c^3\end{array}\right]}^T\\ r_{\mathrm{wc}}2=r_{\mathrm{wc}}3\×r_{\mathrm{wc}}1& \end{array}\right.---\left(14\right)$

Here r _wc1, r _wc2, r _wc3 are respectively R _wccolumn vector.

4, external parameter is solved:

Pass through in any one group of data, the transformational relation R between radar and camera coordinates system can be solved _lc, T _lc:

$\left\{\begin{array}{c}{R}_{\mathrm{lc}}={R_{{\mathrm{wc}}_i}R}_{{\mathrm{wl}}_i}^{-1}\\ T_{\mathrm{lc}}=T_{{\mathrm{wc}}_i}-R_{{\mathrm{wc}}_i}{R}_{{\mathrm{wl}}_i}^{-1}{T}_{{\mathrm{wl}}_i}\end{array},i=\mathrm{1,2}\right.---\left(15\right)$

Further, coplanarity trihedral corresponding surface put under radar fix system and camera coordinates system is utilized, according to objective function F ₄to R _lc, T _lccarry out nonlinear optimization, the R after being optimized _lc, T _lc.

$F_4(R_{\mathrm{lc}},T_{\mathrm{lc}})=\arg \underset{R_{\mathrm{lc}},T_{\mathrm{lc}}}{\min }\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{c_i}^{\mathrm{jT}}(R_{\mathrm{lc}}{P}_{l_i}^{j,k}+T_{\mathrm{lc}})-d_{c_i}^j\left|\right|}^2---\left(16\right)$

Hereto, the calibrating external parameters of radar-camera system is complete.

Claims (1)

1., based on radar-camera system method for calibrating external parameters of any trihedral, it is characterized in that the method comprises the following steps:

(1) utilize radar-camera system, two frame data are gathered to the demarcation scene that there is any trihedral, and according to trihedral definition world coordinate system O _w-X _wy _wz _w, there is relative motion between two frame data of collection, and the coloured image of the three-dimensional point cloud having corresponding radar to gather in every frame data and collected by camera;

(2) radar system parameters is solved: in three-dimensional point cloud, mark the point on trihedral three faces, be expressed as:

$P_l=\{P_{l_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_{l_i}^j\}$

Wherein subscript l represents these points at radar fix system O _l-X _ly _lz _lunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth point, and the number of each upper point is point parameter on same plane represent:

$N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j=0$

Wherein for the normal vector of each, for the distance to coordinate origin;

Then according to the coplanarity of trihedral at radar two frame data mid point, to two frames totally 6 faces carry out nonlinear optimization;

Pass through obtain rotation matrix and translation vector that world coordinates in two frame data is tied to radar fix system i=1,2, and obtain the kinematic parameter of radar-camera system under radar fix system between two frames thus, namely the first frame is relative to the rotation matrix of the second frame and translation vector

(3) camera system parameters is solved: utilize the corresponding region in SIFT algorithm trihedral three faces in front and back two two field picture of collected by camera to extract the unique point of front and back frame coupling, be expressed as:

$P_c=\{P_{c_i}^{j,k},i=\mathrm{1,2},j=\mathrm{1,2,3},k=n_c^j\}$

Wherein subscript c represents these points at camera coordinates system O _c-X _cy _cz _cunder, i represents the i-th frame data, and j represents a jth face, and k represents a kth unique point, and the front and back two obverse matching characteristic of frame is counted out and is equally

According to P _ccalculate the essential matrix E of two frame motions, and obtain the kinematic parameter of radar-camera system under camera coordinates system between two frames further wherein for the first frame under camera coordinates system is relative to the rotation matrix of the second frame, for translation vector normalized result:

$t_{c_1{c}_2}=T_{c_1{c}_2}/\left|\right|T_{c_1{c}_2}\left|\right|,\left|\right|t_{c_1{c}_2}\left|\right|=1$

Utilize parameter under initialization camera coordinates system , herein for the parameter of each plane of trihedral under camera coordinates system; According to the corresponding projection relation of the unique point extracted, right be optimized;

Pass through obtain world coordinates in two frame data and be tied to the transformational relation of camera coordinates system, be i.e. rotation matrix and translation vector i=1,2;

(4) external parameter is solved: utilize R _wc, T _wc, R _wl, T _wlobtain the transformational relation that radar fix is tied to camera coordinates system, i.e. rotation matrix R _lcwith translation vector T _lc; Utilize coplanarity trihedral put under radar fix system and camera coordinates system, to R _lc, T _lccarry out nonlinear optimization, the transformational relation R after being optimized _lc, T _lc;

The concrete grammar that described step (2) solves radar system parameters is:

Utilize linear least-squares optimization object function F ₁, to the point on an i-th frame data jth face carry out plane fitting, obtain parameter

$F_1(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_i}^{\mathrm{jT}}{P}_{l_i}^j-d_{l_i}^j\left|\right|}^2$

Pass through obtain the transformational relation that world coordinates is tied to radar fix system i=1,2:

$\left\{\begin{array}{c}{r}_{\mathrm{wl}}1=\frac{N_l^1\×N_l^3}{\left|\right|N_l^1\×N_l^3\left|\right|}\\ r_{\mathrm{wl}}3=N_l^3,T_l={\left[\begin{array}{ccc}{N}_l^1& N_l^2& N_l^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_l^1& d_l^2& d_l^3\end{array}\right]}^T\\ r_{\mathrm{wl}}2=r_{\mathrm{wl}}3\×r_{\mathrm{wl}}1\end{array}\right.$

Here r _wl1, r _wl2, r _wl3 are respectively R _wlcolumn vector;

Pass through i=1,2 to obtain under radar fix system the first frame relative to the kinematic parameter of the second frame $R_{l_1{l}_2},T_{l_1{l}_2}:$

$\left\{\begin{array}{c}{R}_{l_1{l}_2}=R_{w{l}_2}^{-1}{R}_{w{l}_1}\\ T_{l_1{l}_2}=T_{w{l}_2}-R_{l_1{l}_2}{T}_{w{l}_1}\end{array}\right.$

According to the coplanarity that corresponding surface in two frame data is put, to all carry out nonlinear optimization, objective function is expressed as:

$F_2(N_{l_i}^j,d_{l_i}^j)=\arg \underset{N_{l_i}^j,d_{l_i}^j}{\min }\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_2}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_1}^{\mathrm{jT}}(R_{l_2{l}_1}{P}_{l_2}^{j,k}+T_{l_2{l}_1})-d_{l_1}^j\left|\right|}^2+\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_1}^j}{\mathrm{\Σ}}}{\left|\right|N_{l_2}^{\mathrm{jT}}(R_{l_1{l}_2}{P}_{l_1}^{j,k}+T_{l_1{l}_2})-d_{l_2}^j\left|\right|}^2$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector; Utilize after optimizing right upgrade;

The concrete grammar that described step (3) solves camera system parameters is:

Two-dimensional points P on image _cwith camera coordinates system lower peripheral surface point P _s,ctransformational relation be:

P _c＝G(P _s,c)

G () is corresponding projection function herein, is determined by known camera type and internal reference;

According to the epipolar geometry constraints of front and back frame corresponding point, obtain:

G ^-1(P _s,c1) ^TEG ^-1(P _s,c2)＝0

Here E is essential matrix; Existing maturation method is utilized to solve essential matrix, and the motion under decompositing camera coordinates system between two frames wherein for normalized translation vector:

$t_{c_1{c}_2}=T_{c_1{c}_2}/\left|\right|T_{c_1{c}_2}\left|\right|,\left|\right|t_{c_1{c}_2}\left|\right|=1$

Because the positional distance of radar and camera in radar-camera system is comparatively near, the rotation between coordinate system is less, so according to parameter to be asked under parameter initial estimation camera coordinates system under radar fix system:

$\left\{\begin{array}{c}\{N_{c_i}^j,d_{c_i}^j\}=\{N_{l_i}^j,d_{l_i}^j\}\\ \left|\right|T_{c_1{c}_2}\left|\right|=\left|\right|T_{l_1{l}_2}\left|\right|\end{array}\right.$

Then, matching characteristic point on image is utilized to utilize re-projection error after recovering three-dimensional coordinate on another two field picture is optimized

$F_3(N_{c_1},d_{c_1},N_{c_2},d_{c_2},R_{c_1{c}_2},T_{c_1{c}_2})=\arg \underset{R_{c_1{c}_2},T_{c_1{c}_2}}{\underset{N_{c_2},d_{c_2}}{\underset{N_{c_1},d_{c_1}}{\min }}}\underset{k=1}{\overset{n_c^j}{\mathrm{\Σ}}}{\underset{j=1}{\overset{3}{\mathrm{\Σ}}}}_{+{|p_{c_1}^{j,k}-G(R_{c_2{c}_1}\frac{d_{c_2}{G}^{-1}\left(p_{c_2}^{j,k}\right)}{N_{c_2}^{j,T}{G}^{-1}\left(p_{c_2}^{j,k}\right)}+T_{c_2{c}_1})|}^2}^{{|p_{c_2}^{j,k}-G(R_{c_1{c}_2}\frac{d_{c_1}{G}^{-1}\left(p_{c_1}^{j,k}\right)}{N_{c_1}^{j,T}{G}^{-1}\left(p_{c_1}^{j,k}\right)}+T_{c_1{c}_2})|}^2}$

Wherein, in formula be the second frame relative to the rotation matrix of the first frame and translation vector;

Pass through obtain the transformational relation that world coordinates is tied to camera coordinates system i=1,2:

$\left\{\begin{array}{c}{r}_{\mathrm{wc}}1=\frac{N_c^1\×N_c^3}{\left|\right|N_c^1\×N_c^3\left|\right|}\\ r_{\mathrm{wc}}3=N_c^3,T_c={\left[\begin{array}{ccc}{N}_c^1& N_c^2& N_c^3\end{array}\right]}^{-T}{\left[\begin{array}{ccc}{d}_c^1& d_c^2& d_c^3\end{array}\right]}^T\\ r_{\mathrm{wc}}2=r_{\mathrm{wc}}3\×r_{\mathrm{wc}}1\end{array}\right.$

Here r _wc1, r _wc2, r _wc3 are respectively R _wccolumn vector;

The concrete grammar that described step (4) solves external parameter is:

Pass through i=1, any one group of data in 2, solve the transformational relation R between radar and camera coordinates system _lc, T _lc:

$\left\{\begin{array}{c}{R}_{\mathrm{lc}}=R_{w{c}_i}{R}_{w{l}_i}^{-1}\\ T_{\mathrm{lc}}=T_{w{c}_i}-R_{w{c}_i}{R}_{w{l}_i}^{-1}{T}_{w{l}_i}\end{array}\right.,i=\mathrm{1,2}$

Further, coplanarity trihedral corresponding surface put under radar fix system and camera coordinates system is utilized, according to objective function F ₄to R _lc, T _lccarry out nonlinear optimization, the R after being optimized _lc, T _lc;

$F_4(R_{\mathrm{lc}},T_{\mathrm{lc}})=\arg \underset{R_{\mathrm{lc}},T_{\mathrm{lc}}}{\min }\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\underset{j=1}{\overset{3}{\mathrm{\Σ}}}\underset{k=1}{\overset{n_{l_i}^j}{\mathrm{\Σ}}}{\left|\right|N_{c_i}^{\mathrm{jT}}(R_{\mathrm{lc}}{P}_{l_i}^{j,k}+T_{\mathrm{lc}})-d_{c_i}^j\left|\right|}^2.$