Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a method and a system for calibrating a camera and a millimeter wave radar of a roadside sensing system in a combined manner.
According to the camera and millimeter wave radar combined calibration method of the roadside sensing system, provided by the invention, the method comprises the following steps:
step S1: establishing projection from a GPS to a world coordinate system;
step S2: calibrating a camera for world coordinates;
step S3: calibrating a world coordinate system from a radar;
step S4: the world coordinates are converted to GPS coordinates.
Preferably, the step S1 includes the steps of:
step S1.1: establishing a world coordinate system;
step S1.2: the GPS coordinates are projected into the world coordinate system.
Preferably, the establishing of the world coordinate system in step S1.1 specifically includes using the ground right below the camera as an origin P0, using the longitude and latitude of the point P0 as the Xw axis and Yw axis of the world coordinate, and using the point P0 as the Zw axis vertically above the ground.
Preferably, the step S2 includes the steps of:
step S2.1: calibrating an internal reference;
step S2.2: and (5) calibrating external reference.
Preferably, the radar coordinates in step S3 are obtained by means of a reflection target; the conversion between the radar coordinate system and the world coordinate is three-dimensional coordinate rigid body transformation.
The invention also provides a camera and millimeter wave radar combined calibration system of the roadside sensing system, which comprises the following modules:
module M1: establishing projection from a GPS to a world coordinate system;
module M2: calibrating a camera for world coordinates;
module M3: calibrating a world coordinate system from a radar;
module M4: the world coordinates are converted to GPS coordinates.
Preferably, the module M1 includes the following modules:
module M1.1: establishing a world coordinate system;
module M1.2: the GPS coordinates are projected into the world coordinate system.
Preferably, the world coordinate system established in the module M1.1 is specifically that the ground right below the camera is taken as an origin P0, the latitude and longitude lines of the point P0 are taken as the Xw axis and Yw axis of the world coordinate, and the point P0 is taken as the Zw axis vertically above the ground.
Preferably, the module M2 includes the following modules:
module M2.1: calibrating an internal reference;
module M2.2: and (5) calibrating external reference.
Preferably, the radar coordinates in the module M3 are obtained by means of a reflection target; the conversion between the radar coordinate system and the world coordinate is three-dimensional coordinate rigid body transformation.
Compared with the prior art, the invention has the following beneficial effects:
1. according to the invention, a world coordinate system is established through GPS approximate projection, and the high-precision RTK equipment can be used for specific implementation, so that the measurement precision and the operation convenience are improved;
2. the invention realizes the mutual conversion of radar coordinates, pixel coordinates and GPS coordinates, and facilitates the roadside sensing system to directly output the GPS coordinates of the detection target;
3. the GPS elevation information is introduced, calibration can be carried out on straight roads and gradient roads, the installation positions and angles of the camera and the radar are flexible, and the applicability of the roadside sensing system is improved.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
The invention provides a high-precision GPS coordinate-based joint calibration method applicable to engineering implementation of cameras and millimeter wave radars on straight roads and gradient roads, which can directly output GPS information of a sensing target after sensing fusion; the method specifically comprises the following steps:
step S1: establishing projection from a GPS to a world coordinate system; step S1.1: establishing a world coordinate system; the ground right below the camera is taken as an origin P0, the latitude and longitude of a P0 point are taken as Xw axes and Yw of world coordinates, and a P0 point is taken as a Zw axis vertically above the ground. Step S1.2: projection of GPS coordinates into a world coordinate system; the high precision GPS coordinates can be obtained by RTK. Taking a certain point P1 on the ground as an example, RTK is used to measure P0 GPS coordinates (longitude, latitude, elevation) as (P0_ lo, P0_ la, P0_ h) and P1 GPS coordinates as (P1_ lo, P1_ la, P1_ h).
According to the triangular relation, the approximate projection coordinates of the GPS coordinates of the point P1 in the world coordinate system are obtained as follows:
equation 1: xw1 ═ f1(P1_ la, P0_ lo)
Equation 2: yw1 ═ f2(P1_ la, P0_ la)
Equation 3: zw1 ═ f3(P1_ h, P0_ h)
I.e., the world coordinate of point P1 is (Xw1, Yw1, Zw1)
Therefore, RTK can be used for acquiring the coordinates of each measuring point of the road surface in a world coordinate system.
Step S2: calibrating a camera; there are four coordinate systems in the camera, respectively world, camera, image, pixel.
The camera calibration is to calculate the internal and external parameters and distortion parameters of the camera, and realize the conversion from world coordinates to image coordinates.
The conversion formula of the pixel coordinates (u, v) to the world coordinates (x, y, z) is as follows:
equation 4:
wherein: s is a scaling factor;
is an internal reference matrix of the camera;
is the external parameter matrix RT of the camera.
Step S2.1: calibrating an internal reference; obtained by using a checkerboard-Zhang Zhengyou calibration algorithm to obtain (f)x,fy,cx,cy) I.e. the first matrix to the right of equation 4.
Step S2.2: calibrating external parameters; the RT matrix can be obtained by PNP estimation, the second matrix to the right of equation 4.
A Perspective-n-Point is a method for estimating the camera position by the world coordinates and pixel coordinates of n sets of given points.
Therefore, after the camera internal and external parameters are obtained through calibration, the conversion between the pixel coordinate and the world coordinate can be realized according to the formula 4.
Step S3: calibrating a radar to a world coordinate system; the radar coordinates can be obtained by means of a reflecting target such as an angular reflection (vehicle). Conversion between the radar coordinate system and world coordinates is converted into a three-dimensional coordinate rigid body, and the corresponding point coordinates of a point P in the coordinate system A in the coordinate system B can be calculated by only calculating a conversion matrix, namely:
PB=T*PA+t
PA: the coordinate of the point P in the coordinate system A;
PB: the coordinate of the point P in the coordinate system B;
t: a 3x3 transformation matrix;
t: a displacement of 3x1 transforms the vector.
Using the SVD calculation method, T and T, i.e. the RT matrix, can be calculated given at least 3 pairs of points.
Therefore, the mutual conversion from the radar coordinate to the world coordinate can be realized.
Step S4: converting world coordinates into GPS coordinates; taking a certain point P1 on the ground as an example, knowing the world coordinate of point P1 and the GPS coordinate of point P0 according to equations 1, 2 and 3, the GPS coordinate of point P1 can be deduced.
The implementation process of the invention is as follows:
1. point selection
Taking five points in total, and taking four points of P1, P2, P3 and P4 on the road surface; the world coordinate system origin P0 is taken directly below the camera.
2. Measuring
2.1, P1, P2, P3, P4 each point acquires three sets of data:
(1) radar coordinate system data (Xradar, Yradar, 0), radar coordinates of reflectors such as corner reflectors (vehicles) at corresponding points on the road surface are measured:
P1(Xradar,Yradar,0)、P2(Xradar,Yradar,0)、P3(Xradar,Yradar,0)、P4(Xradar,Yradar,0)。
(2) and acquiring GPS coordinates of each point on the road surface by using RTK, wherein the GPS elevation information of all points on the straight road surface can be uniformly set to be 0. And (3) taking elevation information actually measured by RTK on a slope road surface:
P1(lo,la,h)、P2(lo,la,h)、P3(lo,la,h)、P4(lo,la,h)。
(3) and (3) acquiring pixel coordinates of each point on the road surface through a camera capture:
P1(u,v)、P2(u,v)、P3(u,v)、P4(u,v)。
2.2, Point P0 GPS coordinates P0(lo, la, h)
The world coordinate of point P0 is (0, 0, H), and H is the camera mounting height.
3. Calculating the conversion of P1, P2, P3, P4 GPS coordinates to world coordinates
Taking P1, P2, P3 and P4 GPS coordinates and P0 point GPS coordinates:
p1(lo, la, h), P2(lo, la, h), P3(lo, la, h), P4(lo, la, h), P0(lo, la, h); with reference to formula 1, formula 2, and formula 3, the world coordinates of P1, P2, P3, and P4 are calculated, that is:
P1(Xworld,Yworld,Zworld)、P2(Xworld,Yworld,Zworld)、P3(Xworld,Yworld,Zworld)、P4(Xworld,Yworld,Zworld)。
4. conversion of computed radar coordinates to world coordinates
Taking the radar coordinates of P1, P2, P3 and P4:
P1(Xradar,Yradar,0)、P2(Xradar,Yradar,0)、P3(Xradar,Yradar,0)、P4(Xradar,Yradar,0)。
taking world coordinates of P1, P2, P3 and P4:
p1(Xworld, Yworld, zwold), P2(Xworld, Yworld, zwold), P3(Xworld, Yworld, zwold), P4(Xworld, Yworld, zwold); and obtaining an RT matrix from a radar coordinate system to a world coordinate system based on an SVD calculation method.
5. And (3) calculating external parameters of the camera, and obtaining R _ T from a world coordinate system to a pixel coordinate system:
taking world coordinates of P1, P2, P3 and P4:
P1(Xworld,Yworld,Zworld)、P2(Xworld,Yworld,Zworld)、P3(Xworld,Yworld,Zworld)、P4(Xworld,Yworld,Zworld)。
taking the coordinates of P1, P2, P3 and P4:
p1(u, v), P2(u, v), P3(u, v), P4(u, v); taking camera parameters, and calculating R _ T from a world coordinate system to a pixel coordinate system by using solvePnP.
6. Conversion of computing world coordinates to pixel coordinates
According to equation 4, the conversion of world coordinates to pixel coordinates can be achieved.
The invention also provides a camera and millimeter wave radar combined calibration system of the roadside sensing system, which comprises the following modules: module M1: establishing projection from a GPS to a world coordinate system; module M1.1: establishing a world coordinate system; the ground right below the camera is taken as an origin P0, the latitude and longitude of a P0 point are taken as the Xw axis and Yw of world coordinates, and the axis is the Zw axis when the point P0 is vertically above the ground. Module M1.2: the GPS coordinates are projected into the world coordinate system.
Module M2: calibrating a camera for world coordinates; module M2.1: calibrating an internal reference; module M2.2: and (5) calibrating external reference.
Module M3: calibrating a world coordinate system from a radar; the radar coordinates are obtained by means of a reflection target; the conversion between the radar coordinate system and the world coordinate is three-dimensional coordinate rigid body transformation.
Module M4: the world coordinates are converted to GPS coordinates.
According to the invention, a world coordinate system is established through GPS approximate projection, and the high-precision RTK equipment can be used for specific implementation, so that the measurement precision and the operation convenience are improved; the mutual conversion of radar coordinates, pixel coordinates and GPS coordinates is realized, and the roadside sensing system can directly output the GPS coordinates of the detected target conveniently; the introduced GPS elevation information can be calibrated for straight roads and gradient roads, the installation positions and angles of the camera and the radar are flexible, and the applicability of the roadside sensing system is improved.
Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, units provided by the present invention as pure computer readable program code, the system and its various devices, modules, units provided by the present invention can be fully implemented by logically programming method steps in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system and various devices, modules and units thereof provided by the invention can be regarded as a hardware component, and the devices, modules and units included in the system for realizing various functions can also be regarded as structures in the hardware component; means, modules, units for performing the various functions may also be regarded as structures within both software modules and hardware components for performing the method.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.